diff --git a/.gitignore b/.gitignore index 3ed22f1..fdb80af 100644 --- a/.gitignore +++ b/.gitignore @@ -141,3 +141,13 @@ venv.bak/ .history CLAUDE.md + +################################ +########### C++ BUILD ########## +################################ +cpp/build/ +*.o +*.a +*.so +*.dylib +compile_commands.json diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 822c3f7..460cff3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,14 +12,14 @@ repos: - id: check-toml - repo: https://github.com/astral-sh/ruff-pre-commit - rev: 'v0.12.8' + rev: 'v0.15.7' hooks: - id: ruff types_or: [python, pyi, jupyter] args: [ --fix, --exit-non-zero-on-fix, --preview ] - repo: https://github.com/pre-commit/mirrors-mypy - rev: v1.17.1 + rev: v1.19.1 hooks: - id: mypy args: [--ignore-missing-imports] diff --git a/README.md b/README.md index 77325c2..ac69220 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # InterpolatePy -![Python](https://img.shields.io/badge/python-3.10+-blue) +![Python](https://img.shields.io/badge/python-3.11+-blue) [![PyPI Downloads](https://static.pepy.tech/badge/interpolatepy)](https://pepy.tech/projects/interpolatepy) [![pre-commit](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/pre-commit.yml/badge.svg)](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/pre-commit.yml) [![ci-test](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/test.yml/badge.svg)](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/test.yml) @@ -10,9 +10,9 @@ InterpolatePy provides 20+ algorithms for smooth trajectory generation with precise control over position, velocity, acceleration, and jerk. From cubic splines and B-curves to quaternion interpolation and S-curve motion profiles โ€” everything you need for professional motion control. -**โšก Fast:** Vectorized NumPy operations, ~1ms for 1000-point cubic splines -**๐ŸŽฏ Precise:** Research-grade algorithms with Cยฒ continuity and bounded derivatives -**๐Ÿ“Š Visual:** Built-in plotting for every algorithm +**โšก Fast:** Optional C++ backend with pybind11; pure-Python fallback uses vectorized NumPy +**๐ŸŽฏ Precise:** Research-grade algorithms with Cยฒ continuity and bounded derivatives +**๐Ÿ“Š Visual:** Built-in plotting for every algorithm **๐Ÿ”ง Complete:** Splines, motion profiles, quaternions, and path planning in one library --- @@ -23,7 +23,7 @@ InterpolatePy provides 20+ algorithms for smooth trajectory generation with prec pip install InterpolatePy ``` -**Requirements:** Python โ‰ฅ3.10, NumPy โ‰ฅ2.0, SciPy โ‰ฅ1.15, Matplotlib โ‰ฅ3.10 +**Requirements:** Python โ‰ฅ3.11, NumPy โ‰ฅ2.3, SciPy โ‰ฅ1.16, Matplotlib โ‰ฅ3.10.5
Development Installation @@ -222,12 +222,23 @@ plt.show() ## Performance & Quality -- **Fast:** Vectorized NumPy operations, optimized algorithms -- **Reliable:** 85%+ test coverage, continuous integration -- **Modern:** Python 3.10+, strict typing, dataclass-based APIs +- **Fast:** Optional C++ backend (Eigen + pybind11) for maximum performance; pure-Python fallback uses vectorized NumPy +- **Reliable:** 85%+ test coverage, continuous integration, 142 additional C++ unit tests +- **Modern:** Python 3.11+, strict typing, dataclass-based APIs - **Research-grade:** Peer-reviewed algorithms from robotics literature -**Typical Performance:** +**C++ Backend:** + +InterpolatePy includes an optional compiled C++ extension for performance-critical workloads. The API is identical regardless of backend: + +```python +import interpolatepy +print(f"C++ backend: {interpolatepy.HAS_CPP}") # True if extension is available +``` + +Set `INTERPOLATEPY_NO_CPP=1` to force pure-Python mode. + +**Typical Performance (pure-Python):** - Cubic spline (1000 points): ~1ms - B-spline evaluation (10k points): ~5ms - S-curve trajectory planning: ~0.5ms @@ -256,6 +267,8 @@ ruff format interpolatepy/ ruff check interpolatepy/ mypy interpolatepy/ +# Run all pre-commit hooks +pre-commit run --all-files ```
diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt new file mode 100644 index 0000000..06aad54 --- /dev/null +++ b/cpp/CMakeLists.txt @@ -0,0 +1,169 @@ +cmake_minimum_required(VERSION 3.21) +project(InterpolateCpp + VERSION 0.1.0 + LANGUAGES CXX + DESCRIPTION "C++ port of InterpolatePy trajectory planning library" +) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# Options +option(INTERPOLATECPP_BUILD_TESTS "Build tests" ON) +option(INTERPOLATECPP_BUILD_BINDINGS "Build pybind11 bindings" OFF) +option(INTERPOLATECPP_BUILD_EXAMPLES "Build examples" OFF) + +# Version macros +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/version.hpp.in" + "${CMAKE_CURRENT_BINARY_DIR}/include/interpolatecpp/version.hpp" +) + +# Dependencies via FetchContent +include(FetchContent) + +FetchContent_Declare( + Eigen + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3.4.0 + GIT_SHALLOW TRUE + SYSTEM +) + +if(INTERPOLATECPP_BUILD_TESTS) + FetchContent_Declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG v3.7.1 + GIT_SHALLOW TRUE + ) +endif() + +if(INTERPOLATECPP_BUILD_BINDINGS) + FetchContent_Declare( + pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + GIT_TAG v2.13.6 + GIT_SHALLOW TRUE + ) +endif() + +# Make Eigen available (suppress its tests and docs) +set(BUILD_TESTING OFF CACHE BOOL "" FORCE) +set(EIGEN_BUILD_TESTING OFF CACHE BOOL "" FORCE) +set(EIGEN_BUILD_DOC OFF CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(Eigen) +set(BUILD_TESTING ON CACHE BOOL "" FORCE) + +# Library sources +set(INTERPOLATECPP_SOURCES + src/cubic_spline.cpp + src/cubic_smoothing_spline.cpp + src/cubic_spline_with_acc1.cpp + src/cubic_spline_with_acc2.cpp + src/smoothing_search.cpp + # Phase 2: B-spline + src/bspline.cpp + src/cubic_bspline_interpolation.cpp + src/bspline_interpolator.cpp + src/approximation_bspline.cpp + src/smoothing_cubic_bspline.cpp + # Phase 3: Motion + src/polynomial_trajectory.cpp + src/double_s_trajectory.cpp + src/trapezoidal_trajectory.cpp + src/parabolic_blend_trajectory.cpp + # Phase 4: Quaternion + src/quaternion.cpp + src/quaternion_spline.cpp + src/squad_c2.cpp + src/log_quaternion_interpolation.cpp + src/modified_log_quaternion_interpolation.cpp + # Phase 5: Path + src/linear_path.cpp + src/circular_path.cpp + src/frenet_frame.cpp + src/linear_traj.cpp +) + +add_library(interpolatecpp ${INTERPOLATECPP_SOURCES}) +add_library(interpolatecpp::interpolatecpp ALIAS interpolatecpp) + +target_include_directories(interpolatecpp + PUBLIC + $ + $ + $ +) + +target_link_libraries(interpolatecpp PUBLIC Eigen3::Eigen) + +set_target_properties(interpolatecpp PROPERTIES + POSITION_INDEPENDENT_CODE ON + CXX_VISIBILITY_PRESET hidden + VISIBILITY_INLINES_HIDDEN ON +) + +target_compile_options(interpolatecpp PRIVATE + $<$: + -Wall -Wextra -Wpedantic -Wconversion -Wshadow + > + $<$: + /W4 + > +) + +# Tests +if(INTERPOLATECPP_BUILD_TESTS) + enable_testing() + FetchContent_MakeAvailable(Catch2) + add_subdirectory(tests) +endif() + +# Bindings +if(INTERPOLATECPP_BUILD_BINDINGS) + FetchContent_MakeAvailable(pybind11) + add_subdirectory(bindings) +endif() + +# Examples +if(INTERPOLATECPP_BUILD_EXAMPLES) + add_subdirectory(examples) +endif() + +# Install rules +include(GNUInstallDirs) + +install(TARGETS interpolatecpp + EXPORT InterpolateCppTargets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install(DIRECTORY include/interpolatecpp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/include/interpolatecpp + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(EXPORT InterpolateCppTargets + FILE InterpolateCppTargets.cmake + NAMESPACE interpolatecpp:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InterpolateCpp +) + +include(CMakePackageConfigHelpers) +configure_package_config_file( + cmake/InterpolateCppConfig.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/InterpolateCppConfig.cmake" + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InterpolateCpp +) + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/InterpolateCppConfig.cmake" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/InterpolateCpp +) diff --git a/cpp/bindings/CMakeLists.txt b/cpp/bindings/CMakeLists.txt new file mode 100644 index 0000000..482d307 --- /dev/null +++ b/cpp/bindings/CMakeLists.txt @@ -0,0 +1,26 @@ +pybind11_add_module(interpolatecpp_py + module.cpp + # Phase 1: Cubic Splines + bind_tridiagonal.cpp + bind_cubic_spline.cpp + bind_smoothing_spline.cpp + bind_acc_splines.cpp + bind_smoothing_search.cpp + # Phase 2: B-Splines + bind_bspline.cpp + # Phase 3: Motion Profiles + bind_motion.cpp + # Phase 4: Quaternion Interpolation + bind_quaternion.cpp + # Phase 5: Geometric Paths + bind_paths.cpp +) + +target_link_libraries(interpolatecpp_py + PRIVATE + interpolatecpp::interpolatecpp +) + +install(TARGETS interpolatecpp_py + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} +) diff --git a/cpp/bindings/bind_acc_splines.cpp b/cpp/bindings/bind_acc_splines.cpp new file mode 100644 index 0000000..fc8d68f --- /dev/null +++ b/cpp/bindings/bind_acc_splines.cpp @@ -0,0 +1,96 @@ +#include +#include +#include + +#include +#include + +namespace py = pybind11; +using namespace interpolatecpp::spline; + +void bind_acc_splines(py::module_& m) { + // SplineParameters + py::class_(m, "SplineParameters") + .def(py::init([](double v0, double vn, std::optional a0, std::optional an, + bool debug) { + return SplineParameters{v0, vn, a0, an, debug}; + }), + py::arg("v0") = 0.0, py::arg("vn") = 0.0, py::arg("a0") = std::nullopt, + py::arg("an") = std::nullopt, py::arg("debug") = false) + .def_readwrite("v0", &SplineParameters::v0) + .def_readwrite("vn", &SplineParameters::vn) + .def_readwrite("a0", &SplineParameters::a0) + .def_readwrite("an", &SplineParameters::an) + .def_readwrite("debug", &SplineParameters::debug); + + // CubicSplineWithAcceleration1 + py::class_(m, "CubicSplineWithAcceleration1") + .def(py::init([](std::vector t, std::vector q, double v0, double vn, + double a0, double an, bool debug) { + return CubicSplineWithAcceleration1(t, q, v0, vn, a0, an, debug); + }), + py::arg("t_points"), py::arg("q_points"), py::arg("v0") = 0.0, + py::arg("vn") = 0.0, py::arg("a0") = 0.0, py::arg("an") = 0.0, + py::arg("debug") = false) + .def("evaluate", + py::overload_cast(&CubicSplineWithAcceleration1::evaluate, py::const_), + py::arg("t")) + .def("evaluate", + py::overload_cast( + &CubicSplineWithAcceleration1::evaluate, py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast(&CubicSplineWithAcceleration1::evaluate_velocity, + py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast( + &CubicSplineWithAcceleration1::evaluate_velocity, py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast(&CubicSplineWithAcceleration1::evaluate_acceleration, + py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast( + &CubicSplineWithAcceleration1::evaluate_acceleration, py::const_), + py::arg("t")) + .def_property_readonly("t_points", &CubicSplineWithAcceleration1::t_points) + .def_property_readonly("q_points", &CubicSplineWithAcceleration1::q_points) + .def_property_readonly("omega", &CubicSplineWithAcceleration1::omega) + .def_property_readonly("n_points", &CubicSplineWithAcceleration1::n_points) + .def_property_readonly("n_orig", &CubicSplineWithAcceleration1::n_orig); + + // CubicSplineWithAcceleration2 + py::class_(m, "CubicSplineWithAcceleration2") + .def(py::init([](std::vector t, std::vector q, SplineParameters params) { + return CubicSplineWithAcceleration2(t, q, params); + }), + py::arg("t_points"), py::arg("q_points"), + py::arg("params") = SplineParameters{}) + .def("evaluate", + py::overload_cast(&CubicSplineWithAcceleration2::evaluate, py::const_), + py::arg("t")) + .def("evaluate", + py::overload_cast( + &CubicSplineWithAcceleration2::evaluate, py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast(&CubicSplineWithAcceleration2::evaluate_velocity, + py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast( + &CubicSplineWithAcceleration2::evaluate_velocity, py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast(&CubicSplineWithAcceleration2::evaluate_acceleration, + py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast( + &CubicSplineWithAcceleration2::evaluate_acceleration, py::const_), + py::arg("t")) + .def_property_readonly("has_quintic_first", &CubicSplineWithAcceleration2::has_quintic_first) + .def_property_readonly("has_quintic_last", &CubicSplineWithAcceleration2::has_quintic_last); +} diff --git a/cpp/bindings/bind_bspline.cpp b/cpp/bindings/bind_bspline.cpp new file mode 100644 index 0000000..d178e4c --- /dev/null +++ b/cpp/bindings/bind_bspline.cpp @@ -0,0 +1,116 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace interpolatecpp::bspline; + +void bind_bspline(py::module_& m) { + auto bspline_mod = m.def_submodule("bspline", "B-spline interpolation algorithms"); + + // Parameterization enum + py::enum_(bspline_mod, "Parameterization") + .value("EquallySpaced", Parameterization::EquallySpaced) + .value("ChordLength", Parameterization::ChordLength) + .value("Centripetal", Parameterization::Centripetal); + + // BSpline base class + py::class_(bspline_mod, "BSpline") + .def(py::init([](int degree, std::vector knots, + const Eigen::MatrixXd& control_points) { + return BSpline(degree, std::span(knots), control_points); + }), + py::arg("degree"), py::arg("knots"), py::arg("control_points")) + .def("evaluate", &BSpline::evaluate, py::arg("u")) + .def("evaluate_derivative", &BSpline::evaluate_derivative, py::arg("u"), + py::arg("order") = 1) + .def("generate_curve_points", &BSpline::generate_curve_points, + py::arg("num_points") = 100) + .def("find_knot_span", &BSpline::find_knot_span, py::arg("u")) + .def_property_readonly("degree", &BSpline::degree) + .def_property_readonly("knots", &BSpline::knots) + .def_property_readonly("control_points", &BSpline::control_points) + .def_property_readonly("u_min", &BSpline::u_min) + .def_property_readonly("u_max", &BSpline::u_max) + .def_property_readonly("dimension", &BSpline::dimension) + .def_property_readonly("n_control_points", &BSpline::n_control_points) + .def_static("create_uniform_knots", &BSpline::create_uniform_knots, py::arg("degree"), + py::arg("num_control_points"), py::arg("domain_min") = 0.0, + py::arg("domain_max") = 1.0) + .def_static("create_periodic_knots", &BSpline::create_periodic_knots, + py::arg("degree"), py::arg("num_control_points"), + py::arg("domain_min") = 0.0, py::arg("domain_max") = 1.0) + .def("basis_functions", &BSpline::basis_functions, py::arg("u"), + py::arg("span_index")) + .def("basis_function_derivatives", &BSpline::basis_function_derivatives, + py::arg("u"), py::arg("span_index"), py::arg("order")); + + // CubicBSplineInterpolation + py::class_(bspline_mod, "CubicBSplineInterpolation") + .def(py::init&, + const std::optional&, Parameterization, bool>(), + py::arg("points"), py::arg("v0") = std::nullopt, py::arg("vn") = std::nullopt, + py::arg("method") = Parameterization::ChordLength, + py::arg("auto_derivatives") = false) + .def_property_readonly("interpolation_points", + &CubicBSplineInterpolation::interpolation_points) + .def_property_readonly("u_bars", &CubicBSplineInterpolation::u_bars); + + // BSplineInterpolator + py::class_(bspline_mod, "BSplineInterpolator") + .def(py::init&, + const std::optional&, + const std::optional&, + const std::optional&, + const std::optional&, bool>(), + py::arg("degree"), py::arg("points"), py::arg("times") = std::nullopt, + py::arg("initial_velocity") = std::nullopt, + py::arg("final_velocity") = std::nullopt, + py::arg("initial_acceleration") = std::nullopt, + py::arg("final_acceleration") = std::nullopt, py::arg("cyclic") = false) + .def_property_readonly("interp_points", &BSplineInterpolator::interp_points) + .def_property_readonly("times", &BSplineInterpolator::times); + + // ApproximationBSpline + py::class_(bspline_mod, "ApproximationBSpline") + .def(py::init&, + Parameterization, bool>(), + py::arg("points"), py::arg("num_control_points"), py::arg("degree") = 3, + py::arg("weights") = std::nullopt, + py::arg("method") = Parameterization::ChordLength, py::arg("debug") = false) + .def("calculate_approximation_error", &ApproximationBSpline::calculate_approximation_error) + .def_property_readonly("original_points", &ApproximationBSpline::original_points) + .def_property_readonly("original_parameters", + &ApproximationBSpline::original_parameters); + + // BSplineParams (for SmoothingCubicBSpline config) + py::class_(bspline_mod, "BSplineParams") + .def(py::init<>()) + .def_readwrite("mu", &BSplineParams::mu) + .def_readwrite("weights", &BSplineParams::weights) + .def_readwrite("v0", &BSplineParams::v0) + .def_readwrite("vn", &BSplineParams::vn) + .def_readwrite("method", &BSplineParams::method) + .def_readwrite("enforce_endpoints", &BSplineParams::enforce_endpoints) + .def_readwrite("auto_derivatives", &BSplineParams::auto_derivatives); + + // SmoothingCubicBSpline + py::class_(bspline_mod, "SmoothingCubicBSpline") + .def(py::init(), py::arg("points"), + py::arg("params") = BSplineParams{}) + .def("calculate_approximation_error", + &SmoothingCubicBSpline::calculate_approximation_error) + .def("calculate_total_error", &SmoothingCubicBSpline::calculate_total_error) + .def_property_readonly("approximation_points", + &SmoothingCubicBSpline::approximation_points) + .def_property_readonly("u_bars", &SmoothingCubicBSpline::u_bars) + .def_property_readonly("mu", &SmoothingCubicBSpline::mu) + .def_property_readonly("lambda_param", &SmoothingCubicBSpline::lambda_param); +} diff --git a/cpp/bindings/bind_cubic_spline.cpp b/cpp/bindings/bind_cubic_spline.cpp new file mode 100644 index 0000000..633fa08 --- /dev/null +++ b/cpp/bindings/bind_cubic_spline.cpp @@ -0,0 +1,39 @@ +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace interpolatecpp::spline; + +void bind_cubic_spline(py::module_& m) { + py::class_(m, "CubicSpline") + .def(py::init([](std::vector t, std::vector q, double v0, double vn, + bool debug) { return CubicSpline(t, q, v0, vn, debug); }), + py::arg("t_points"), py::arg("q_points"), py::arg("v0") = 0.0, + py::arg("vn") = 0.0, py::arg("debug") = false) + .def("evaluate", py::overload_cast(&CubicSpline::evaluate, py::const_), + py::arg("t")) + .def("evaluate", + py::overload_cast(&CubicSpline::evaluate, py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast(&CubicSpline::evaluate_velocity, py::const_), py::arg("t")) + .def("evaluate_velocity", + py::overload_cast(&CubicSpline::evaluate_velocity, + py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast(&CubicSpline::evaluate_acceleration, py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast(&CubicSpline::evaluate_acceleration, + py::const_), + py::arg("t")) + .def_property_readonly("t_points", &CubicSpline::t_points) + .def_property_readonly("q_points", &CubicSpline::q_points) + .def_property_readonly("velocities", &CubicSpline::velocities) + .def_property_readonly("coefficients", &CubicSpline::coefficients) + .def_property_readonly("n_segments", &CubicSpline::n_segments); +} diff --git a/cpp/bindings/bind_motion.cpp b/cpp/bindings/bind_motion.cpp new file mode 100644 index 0000000..274ecaf --- /dev/null +++ b/cpp/bindings/bind_motion.cpp @@ -0,0 +1,124 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace interpolatecpp::motion; + +void bind_motion(py::module_& m) { + auto motion_mod = m.def_submodule("motion", "Motion profile algorithms"); + + // Result types + py::class_(motion_mod, "TrajectoryResult") + .def_readonly("position", &TrajectoryResult::position) + .def_readonly("velocity", &TrajectoryResult::velocity) + .def_readonly("acceleration", &TrajectoryResult::acceleration); + + py::class_(motion_mod, "FullTrajectoryResult") + .def_readonly("position", &FullTrajectoryResult::position) + .def_readonly("velocity", &FullTrajectoryResult::velocity) + .def_readonly("acceleration", &FullTrajectoryResult::acceleration) + .def_readonly("jerk", &FullTrajectoryResult::jerk); + + // BoundaryCondition + py::class_(motion_mod, "BoundaryCondition") + .def(py::init<>()) + .def_readwrite("position", &BoundaryCondition::position) + .def_readwrite("velocity", &BoundaryCondition::velocity) + .def_readwrite("acceleration", &BoundaryCondition::acceleration) + .def_readwrite("jerk", &BoundaryCondition::jerk); + + // TimeInterval + py::class_(motion_mod, "TimeInterval") + .def(py::init<>()) + .def_readwrite("start", &TimeInterval::start) + .def_readwrite("end", &TimeInterval::end) + .def("duration", &TimeInterval::duration); + + // StateParams + py::class_(motion_mod, "StateParams") + .def(py::init([](double q0, double q1, double v0, double v1) { + return StateParams{q0, q1, v0, v1}; + }), + py::arg("q_0"), py::arg("q_1"), py::arg("v_0") = 0.0, py::arg("v_1") = 0.0) + .def_readonly("q_0", &StateParams::q_0) + .def_readonly("q_1", &StateParams::q_1) + .def_readonly("v_0", &StateParams::v_0) + .def_readonly("v_1", &StateParams::v_1); + + // TrajectoryBounds + py::class_(motion_mod, "TrajectoryBounds") + .def(py::init(), py::arg("v_bound"), py::arg("a_bound"), + py::arg("j_bound")) + .def_readonly("v_bound", &TrajectoryBounds::v_bound) + .def_readonly("a_bound", &TrajectoryBounds::a_bound) + .def_readonly("j_bound", &TrajectoryBounds::j_bound); + + // PolynomialTrajectory + py::class_(motion_mod, "PolynomialTrajectory") + .def(py::init(), + py::arg("bc_start"), py::arg("bc_end"), py::arg("interval"), py::arg("order")) + .def("evaluate", &PolynomialTrajectory::evaluate, py::arg("t")) + .def_property_readonly("order", &PolynomialTrajectory::order) + .def_property_readonly("t_start", &PolynomialTrajectory::t_start) + .def_property_readonly("t_end", &PolynomialTrajectory::t_end) + .def_property_readonly("duration", &PolynomialTrajectory::duration) + .def_property_readonly("coefficients", &PolynomialTrajectory::coefficients) + .def_static("heuristic_velocities", &PolynomialTrajectory::heuristic_velocities, + py::arg("points"), py::arg("times")) + .def_static("multipoint_trajectory", &PolynomialTrajectory::multipoint_trajectory, + py::arg("points"), py::arg("times"), py::arg("order") = 3, + py::arg("v0") = 0.0, py::arg("vn") = 0.0) + .def_static("evaluate_multipoint", &PolynomialTrajectory::evaluate_multipoint, + py::arg("segments"), py::arg("t")); + + // DoubleSTrajectory + py::class_(motion_mod, "DoubleSTrajectory") + .def(py::init(), py::arg("state"), + py::arg("bounds")) + .def("evaluate", &DoubleSTrajectory::evaluate, py::arg("t")) + .def_property_readonly("duration", &DoubleSTrajectory::duration) + .def("phase_durations", &DoubleSTrajectory::phase_durations); + + // TrapezoidalTrajectory + py::class_(motion_mod, "TrapezoidalTrajectory") + .def(py::init(), + py::arg("q0"), py::arg("q1"), py::arg("amax"), py::arg("vmax"), + py::arg("v0") = 0.0, py::arg("v1") = 0.0, py::arg("t0") = 0.0) + .def(py::init([](double q0, double q1, double amax, double v0, double v1, + double t0, double duration) { + return TrapezoidalTrajectory(TrapezoidalTrajectory::DurationBased{}, q0, + q1, amax, v0, v1, t0, duration); + }), + py::arg("q0"), py::arg("q1"), py::arg("amax"), py::arg("v0"), + py::arg("v1"), py::arg("t0"), py::arg("duration"), + "Duration-based constructor (computes required acceleration).") + .def("evaluate", &TrapezoidalTrajectory::evaluate, py::arg("t")) + .def_property_readonly("duration", &TrapezoidalTrajectory::duration) + .def_property_readonly("t_start", &TrapezoidalTrajectory::t_start) + .def_property_readonly("t_end", &TrapezoidalTrajectory::t_end) + .def_static("heuristic_velocities", &TrapezoidalTrajectory::heuristic_velocities, + py::arg("points"), py::arg("times"), py::arg("vmax")) + .def_static("interpolate_waypoints", &TrapezoidalTrajectory::interpolate_waypoints, + py::arg("points"), py::arg("amax"), py::arg("vmax"), py::arg("v0") = 0.0, + py::arg("vn") = 0.0, py::arg("times") = std::vector{}, + py::arg("velocities") = std::vector{}) + .def_static("evaluate_multipoint", &TrapezoidalTrajectory::evaluate_multipoint, + py::arg("segments"), py::arg("t")); + + // ParabolicBlendTrajectory + py::class_(motion_mod, "ParabolicBlendTrajectory") + .def(py::init&, const std::vector&, + const std::vector&>(), + py::arg("q"), py::arg("t"), py::arg("dt_blend")) + .def("evaluate", &ParabolicBlendTrajectory::evaluate, py::arg("t")) + .def_property_readonly("duration", &ParabolicBlendTrajectory::duration) + .def_property_readonly("n_waypoints", &ParabolicBlendTrajectory::n_waypoints); +} diff --git a/cpp/bindings/bind_paths.cpp b/cpp/bindings/bind_paths.cpp new file mode 100644 index 0000000..e0f3e5c --- /dev/null +++ b/cpp/bindings/bind_paths.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace py = pybind11; +using namespace interpolatecpp::path; + +void bind_paths(py::module_& m) { + auto path_mod = m.def_submodule("path", "Geometric path algorithms"); + + // LinearPath + py::class_(path_mod, "LinearPath") + .def(py::init(), py::arg("pi"), + py::arg("pf")) + .def("position", + py::overload_cast(&LinearPath::position, py::const_), py::arg("s")) + .def("position", + py::overload_cast(&LinearPath::position, py::const_), + py::arg("s")) + .def("velocity", &LinearPath::velocity, py::arg("s")) + .def("acceleration", &LinearPath::acceleration, py::arg("s")) + .def_property_readonly("length", &LinearPath::length); + + // CircularPath + py::class_(path_mod, "CircularPath") + .def(py::init(), + py::arg("axis"), py::arg("axis_point"), py::arg("circle_point")) + .def("position", + py::overload_cast(&CircularPath::position, py::const_), py::arg("s")) + .def("position", + py::overload_cast(&CircularPath::position, py::const_), + py::arg("s")) + .def("velocity", &CircularPath::velocity, py::arg("s")) + .def("acceleration", &CircularPath::acceleration, py::arg("s")) + .def_property_readonly("radius", &CircularPath::radius) + .def_property_readonly("center", &CircularPath::center); + + // FrenetFrame + py::class_(path_mod, "FrenetFrame") + .def_readonly("tangent", &FrenetFrame::tangent) + .def_readonly("normal", &FrenetFrame::normal) + .def_readonly("binormal", &FrenetFrame::binormal) + .def_readonly("curvature", &FrenetFrame::curvature) + .def_readonly("torsion", &FrenetFrame::torsion); + + // compute_frenet_frames + path_mod.def("compute_frenet_frames", &compute_frenet_frames, py::arg("curve"), + py::arg("s_values")); + + // Helper trajectory functions + path_mod.def("circular_trajectory_with_derivatives", + &circular_trajectory_with_derivatives, py::arg("u"), py::arg("r") = 2.0); + path_mod.def("helicoidal_trajectory_with_derivatives", + &helicoidal_trajectory_with_derivatives, py::arg("u"), py::arg("r") = 2.0, + py::arg("d") = 0.5); + + // LinearTrajResult + py::class_(path_mod, "LinearTrajResult") + .def_readonly("positions", &LinearTrajResult::positions) + .def_readonly("velocities", &LinearTrajResult::velocities) + .def_readonly("accelerations", &LinearTrajResult::accelerations); + + // linear_traj + path_mod.def("linear_traj", &linear_traj, py::arg("p0"), py::arg("p1"), py::arg("t0"), + py::arg("t1"), py::arg("num_points") = 100); +} diff --git a/cpp/bindings/bind_quaternion.cpp b/cpp/bindings/bind_quaternion.cpp new file mode 100644 index 0000000..e3a3965 --- /dev/null +++ b/cpp/bindings/bind_quaternion.cpp @@ -0,0 +1,142 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace interpolatecpp::quat; + +void bind_quaternion(py::module_& m) { + auto quat_mod = m.def_submodule("quat", "Quaternion interpolation algorithms"); + + // Quaternion + py::class_(quat_mod, "Quaternion") + .def(py::init(), py::arg("w") = 1.0, + py::arg("x") = 0.0, py::arg("y") = 0.0, py::arg("z") = 0.0) + .def_static("identity", &Quaternion::identity) + .def_static("from_angle_axis", &Quaternion::from_angle_axis, py::arg("angle"), + py::arg("axis")) + .def_static("from_euler_angles", &Quaternion::from_euler_angles, py::arg("roll"), + py::arg("pitch"), py::arg("yaw")) + .def_property_readonly("w", &Quaternion::w) + .def_property_readonly("x", &Quaternion::x) + .def_property_readonly("y", &Quaternion::y) + .def_property_readonly("z", &Quaternion::z) + .def_property_readonly("vec", &Quaternion::vec) + .def("__mul__", + py::overload_cast(&Quaternion::operator*, py::const_)) + .def("__mul__", + py::overload_cast(&Quaternion::operator*, py::const_)) + .def("__rmul__", + [](const Quaternion& q, double s) { return q * s; }) + .def("__add__", + py::overload_cast(&Quaternion::operator+, py::const_)) + .def("__sub__", + py::overload_cast(&Quaternion::operator-, py::const_)) + .def("__neg__", [](const Quaternion& q) { return -q; }) + .def("conjugate", &Quaternion::conjugate) + .def("inverse", &Quaternion::inverse) + .def("unit", &Quaternion::unit) + .def("norm", &Quaternion::norm) + .def("norm_squared", &Quaternion::norm_squared) + .def("dot_product", &Quaternion::dot_product, py::arg("other")) + .def("to_rotation_matrix", &Quaternion::to_rotation_matrix) + .def("to_transformation_matrix", &Quaternion::to_transformation_matrix) + .def("to_axis_angle", &Quaternion::to_axis_angle) + .def("to_euler_angles", &Quaternion::to_euler_angles) + .def_static("from_rotation_matrix", &Quaternion::from_rotation_matrix, + py::arg("rotation_matrix")) + // Dynamics + .def("E", &Quaternion::E, py::arg("sign")) + .def("dot", &Quaternion::dot, py::arg("omega"), py::arg("sign")) + .def_static("Omega", &Quaternion::Omega, py::arg("q"), py::arg("q_dot")) + .def_static("slerp", &Quaternion::slerp, py::arg("q0"), py::arg("q1"), py::arg("t")) + .def_static("slerp_prime", &Quaternion::slerp_prime, py::arg("q0"), + py::arg("q1"), py::arg("t")) + .def_static("squad", &Quaternion::squad, py::arg("p"), py::arg("a"), py::arg("b"), + py::arg("q"), py::arg("t")) + .def_static("compute_intermediate_quaternion", + &Quaternion::compute_intermediate_quaternion, py::arg("q_prev"), + py::arg("q_curr"), py::arg("q_next")) + .def_static("exp", &Quaternion::exp, py::arg("q")) + .def_static("log", &Quaternion::log, py::arg("q")) + .def_static("power", &Quaternion::power, py::arg("q"), py::arg("t")); + + // QuaternionSpline + py::enum_(quat_mod, "QuaternionSplineMethod") + .value("Slerp", QuaternionSpline::Method::Slerp) + .value("Squad", QuaternionSpline::Method::Squad) + .value("Auto", QuaternionSpline::Method::Auto); + + py::class_(quat_mod, "QuaternionSpline") + .def(py::init&, const std::vector&, + QuaternionSpline::Method>(), + py::arg("time_points"), py::arg("quaternions"), + py::arg("method") = QuaternionSpline::Method::Auto) + .def("evaluate", &QuaternionSpline::evaluate, py::arg("t")) + .def("evaluate_velocity", &QuaternionSpline::evaluate_velocity, py::arg("t")) + .def("evaluate_acceleration", &QuaternionSpline::evaluate_acceleration, py::arg("t")) + .def_property_readonly("t_min", &QuaternionSpline::t_min) + .def_property_readonly("t_max", &QuaternionSpline::t_max); + + // SquadC2Config + py::class_(quat_mod, "SquadC2Config") + .def(py::init<>()) + .def_readwrite("time_points", &SquadC2Config::time_points) + .def_readwrite("quaternions", &SquadC2Config::quaternions) + .def_readwrite("normalize_quaternions", &SquadC2Config::normalize_quaternions) + .def_readwrite("validate_continuity", &SquadC2Config::validate_continuity); + + // SquadC2 + py::class_(quat_mod, "SquadC2") + .def(py::init&, const std::vector&, bool, bool>(), + py::arg("time_points"), py::arg("quaternions"), + py::arg("normalize_quaternions") = true, + py::arg("validate_continuity") = true) + .def(py::init(), py::arg("config")) + .def("evaluate", &SquadC2::evaluate, py::arg("t")) + .def("evaluate_velocity", &SquadC2::evaluate_velocity, py::arg("t")) + .def("evaluate_acceleration", &SquadC2::evaluate_acceleration, py::arg("t")) + .def_property_readonly("t_min", &SquadC2::t_min) + .def_property_readonly("t_max", &SquadC2::t_max) + .def_property_readonly("validate_continuity", &SquadC2::validate_continuity); + + // LogQuaternionInterpolation + py::class_(quat_mod, "LogQuaternionInterpolation") + .def(py::init&, const std::vector&, int, + const std::optional&, + const std::optional&>(), + py::arg("time_points"), py::arg("quaternions"), py::arg("degree") = 3, + py::arg("initial_velocity") = std::nullopt, + py::arg("final_velocity") = std::nullopt) + .def("evaluate", &LogQuaternionInterpolation::evaluate, py::arg("t")) + .def("evaluate_velocity", &LogQuaternionInterpolation::evaluate_velocity, py::arg("t")) + .def("evaluate_acceleration", &LogQuaternionInterpolation::evaluate_acceleration, + py::arg("t")) + .def_property_readonly("t_min", &LogQuaternionInterpolation::t_min) + .def_property_readonly("t_max", &LogQuaternionInterpolation::t_max); + + // ModifiedLogQuaternionInterpolation + py::class_(quat_mod, + "ModifiedLogQuaternionInterpolation") + .def(py::init&, const std::vector&, int, bool, + const std::optional&, + const std::optional&>(), + py::arg("time_points"), py::arg("quaternions"), py::arg("degree") = 3, + py::arg("normalize_axis") = true, py::arg("initial_velocity") = std::nullopt, + py::arg("final_velocity") = std::nullopt) + .def("evaluate", &ModifiedLogQuaternionInterpolation::evaluate, py::arg("t")) + .def("evaluate_velocity", &ModifiedLogQuaternionInterpolation::evaluate_velocity, + py::arg("t")) + .def("evaluate_acceleration", + &ModifiedLogQuaternionInterpolation::evaluate_acceleration, py::arg("t")) + .def_property_readonly("t_min", &ModifiedLogQuaternionInterpolation::t_min) + .def_property_readonly("t_max", &ModifiedLogQuaternionInterpolation::t_max) + .def_property_readonly("normalize_axis", + &ModifiedLogQuaternionInterpolation::normalize_axis); +} diff --git a/cpp/bindings/bind_smoothing_search.cpp b/cpp/bindings/bind_smoothing_search.cpp new file mode 100644 index 0000000..b2a1285 --- /dev/null +++ b/cpp/bindings/bind_smoothing_search.cpp @@ -0,0 +1,40 @@ +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace interpolatecpp::spline; + +void bind_smoothing_search(py::module_& m) { + // SplineConfig + py::class_(m, "SplineConfig") + .def(py::init([](std::optional weights, double v0, double vn, + int max_iterations, bool debug) { + return SplineConfig{weights, v0, vn, max_iterations, debug}; + }), + py::arg("weights") = std::nullopt, py::arg("v0") = 0.0, py::arg("vn") = 0.0, + py::arg("max_iterations") = 50, py::arg("debug") = false) + .def_readwrite("weights", &SplineConfig::weights) + .def_readwrite("v0", &SplineConfig::v0) + .def_readwrite("vn", &SplineConfig::vn) + .def_readwrite("max_iterations", &SplineConfig::max_iterations) + .def_readwrite("debug", &SplineConfig::debug); + + // SmoothingSearchResult + py::class_(m, "SmoothingSearchResult") + .def_readonly("spline", &SmoothingSearchResult::spline) + .def_readonly("mu", &SmoothingSearchResult::mu) + .def_readonly("max_error", &SmoothingSearchResult::max_error) + .def_readonly("iterations", &SmoothingSearchResult::iterations); + + // Free function + m.def( + "smoothing_spline_with_tolerance", + [](std::vector t, std::vector q, double tolerance, + const SplineConfig& config) { + return smoothing_spline_with_tolerance(t, q, tolerance, config); + }, + py::arg("t_points"), py::arg("q_points"), py::arg("tolerance"), py::arg("config")); +} diff --git a/cpp/bindings/bind_smoothing_spline.cpp b/cpp/bindings/bind_smoothing_spline.cpp new file mode 100644 index 0000000..8b6ed5f --- /dev/null +++ b/cpp/bindings/bind_smoothing_spline.cpp @@ -0,0 +1,52 @@ +#include +#include +#include + +#include + +#include + +namespace py = pybind11; +using namespace interpolatecpp::spline; + +void bind_smoothing_spline(py::module_& m) { + py::class_(m, "CubicSmoothingSpline") + .def(py::init([](std::vector t, std::vector q, double mu, + std::optional> weights, double v0, double vn, + bool debug) { + std::optional> w_span; + if (weights.has_value()) { + w_span = std::span(weights->data(), weights->size()); + } + return CubicSmoothingSpline(t, q, mu, w_span, v0, vn, debug); + }), + py::arg("t_points"), py::arg("q_points"), py::arg("mu") = 0.5, + py::arg("weights") = std::nullopt, py::arg("v0") = 0.0, py::arg("vn") = 0.0, + py::arg("debug") = false) + .def("evaluate", + py::overload_cast(&CubicSmoothingSpline::evaluate, py::const_), + py::arg("t")) + .def("evaluate", + py::overload_cast(&CubicSmoothingSpline::evaluate, + py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast(&CubicSmoothingSpline::evaluate_velocity, py::const_), + py::arg("t")) + .def("evaluate_velocity", + py::overload_cast( + &CubicSmoothingSpline::evaluate_velocity, py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast(&CubicSmoothingSpline::evaluate_acceleration, py::const_), + py::arg("t")) + .def("evaluate_acceleration", + py::overload_cast( + &CubicSmoothingSpline::evaluate_acceleration, py::const_), + py::arg("t")) + .def_property_readonly("t_points", &CubicSmoothingSpline::t_points) + .def_property_readonly("q_points", &CubicSmoothingSpline::q_points) + .def_property_readonly("s_points", &CubicSmoothingSpline::s_points) + .def_property_readonly("mu", &CubicSmoothingSpline::mu) + .def_property_readonly("coefficients", &CubicSmoothingSpline::coefficients); +} diff --git a/cpp/bindings/bind_tridiagonal.cpp b/cpp/bindings/bind_tridiagonal.cpp new file mode 100644 index 0000000..7eae957 --- /dev/null +++ b/cpp/bindings/bind_tridiagonal.cpp @@ -0,0 +1,13 @@ +#include +#include + +#include + +namespace py = pybind11; + +void bind_tridiagonal(py::module_& m) { + m.def("solve_tridiagonal", &interpolatecpp::solve_tridiagonal, + py::arg("lower_diagonal"), py::arg("main_diagonal"), + py::arg("upper_diagonal"), py::arg("right_hand_side"), + "Solve a tridiagonal system using the Thomas algorithm"); +} diff --git a/cpp/bindings/module.cpp b/cpp/bindings/module.cpp new file mode 100644 index 0000000..e94242d --- /dev/null +++ b/cpp/bindings/module.cpp @@ -0,0 +1,45 @@ +#include + +namespace py = pybind11; + +// Phase 1: Cubic Splines +void bind_tridiagonal(py::module_& m); +void bind_cubic_spline(py::module_& m); +void bind_smoothing_spline(py::module_& m); +void bind_acc_splines(py::module_& m); +void bind_smoothing_search(py::module_& m); + +// Phase 2: B-Splines +void bind_bspline(py::module_& m); + +// Phase 3: Motion Profiles +void bind_motion(py::module_& m); + +// Phase 4: Quaternion Interpolation +void bind_quaternion(py::module_& m); + +// Phase 5: Geometric Paths +void bind_paths(py::module_& m); + +PYBIND11_MODULE(interpolatecpp_py, m) { + m.doc() = "C++ backend for InterpolatePy trajectory planning library"; + + // Phase 1 + bind_tridiagonal(m); + bind_cubic_spline(m); + bind_smoothing_spline(m); + bind_acc_splines(m); + bind_smoothing_search(m); + + // Phase 2 + bind_bspline(m); + + // Phase 3 + bind_motion(m); + + // Phase 4 + bind_quaternion(m); + + // Phase 5 + bind_paths(m); +} diff --git a/cpp/cmake/InterpolateCppConfig.cmake.in b/cpp/cmake/InterpolateCppConfig.cmake.in new file mode 100644 index 0000000..768a9a4 --- /dev/null +++ b/cpp/cmake/InterpolateCppConfig.cmake.in @@ -0,0 +1,5 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/InterpolateCppTargets.cmake") + +check_required_components(InterpolateCpp) diff --git a/cpp/cmake/version.hpp.in b/cpp/cmake/version.hpp.in new file mode 100644 index 0000000..e0a632f --- /dev/null +++ b/cpp/cmake/version.hpp.in @@ -0,0 +1,6 @@ +#pragma once + +#define INTERPOLATECPP_VERSION_MAJOR @PROJECT_VERSION_MAJOR@ +#define INTERPOLATECPP_VERSION_MINOR @PROJECT_VERSION_MINOR@ +#define INTERPOLATECPP_VERSION_PATCH @PROJECT_VERSION_PATCH@ +#define INTERPOLATECPP_VERSION "@PROJECT_VERSION@" diff --git a/cpp/examples/CMakeLists.txt b/cpp/examples/CMakeLists.txt new file mode 100644 index 0000000..b4a79eb --- /dev/null +++ b/cpp/examples/CMakeLists.txt @@ -0,0 +1,29 @@ +set(EXAMPLE_SOURCES + # Phase 1: Cubic Splines + cubic_spline_example.cpp + cubic_spline_acc1_example.cpp + cubic_spline_acc2_example.cpp + cubic_smoothing_example.cpp + # Phase 2: B-Splines + bspline_example.cpp + bspline_cubic_example.cpp + bspline_interpolator_example.cpp + bspline_approx_smooth_example.cpp + # Phase 3: Motion Profiles + trapezoidal_example.cpp + polynomial_example.cpp + double_s_example.cpp + parabolic_linear_example.cpp + # Phase 4: Quaternion + quaternion_example.cpp + # Phase 5: Paths & Concepts + paths_example.cpp + concepts_example.cpp +) + +foreach(source IN LISTS EXAMPLE_SOURCES) + get_filename_component(name ${source} NAME_WE) + add_executable(${name} ${source}) + target_link_libraries(${name} PRIVATE interpolatecpp::interpolatecpp) + target_include_directories(${name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/shared) +endforeach() diff --git a/cpp/examples/bspline_approx_smooth_example.cpp b/cpp/examples/bspline_approx_smooth_example.cpp new file mode 100644 index 0000000..b449cfe --- /dev/null +++ b/cpp/examples/bspline_approx_smooth_example.cpp @@ -0,0 +1,387 @@ +/// B-spline approximation and smoothing example -- C++ port of +/// examples/b_spline_approx_ex.py AND examples/b_spline_smooth_ex.py +/// +/// Part 1: ApproximationBSpline -- least-squares fitting with varying control +/// point counts and degrees. +/// Part 2: SmoothingCubicBSpline -- smoothing with different lambda/mu values +/// (Example 8.12). + +#include +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::bspline; + +// ============================================================================ +// Part 1: Approximation B-spline +// ============================================================================ + +/// Generate sample points from a B-spline defined by the Example 8.10 control points. +static Eigen::MatrixXd generate_sample_points(int num_samples) { + // Control points from Example 8.10 + Eigen::MatrixXd control_points(10, 2); + control_points << 137, 229, + 101, 201, + 177, 121, + 93, 44, + 62, 203, + 49, 272, + 104, 402, + 141, 277, + 147, 258, + 138, 231; + + const int degree = 3; + const Eigen::VectorXd knots = BSpline::create_uniform_knots(degree, 10); + + const BSpline spline(degree, + std::span(knots.data(), static_cast(knots.size())), + control_points); + + const auto [params, curve_pts] = spline.generate_curve_points(num_samples); + return curve_pts; +} + +/// Example from Section 8.5: Approximation with different configurations. +static void example_approximation() { + ex::print_header("Part 1 -- B-spline Approximation (Section 8.5)"); + + // Print the source control points + std::cout << "Control points from Example 8.10:\n"; + const std::vector> cp_coords = { + {137, 229}, {101, 201}, {177, 121}, {93, 44}, {62, 203}, + {49, 272}, {104, 402}, {141, 277}, {147, 258}, {138, 231} + }; + for (size_t i = 0; i < cp_coords.size(); ++i) { + std::cout << " P" << i << ": (" << cp_coords[i].first + << ", " << cp_coords[i].second << ")\n"; + } + + // Generate sample points + const Eigen::MatrixXd sample_points = generate_sample_points(84); + std::cout << "\nGenerated " << sample_points.rows() << " sample points.\n"; + + // Test cases: {num_cps, degree, title} + struct TestCase { + int num_cps; + int degree; + std::string title; + }; + const std::vector test_cases = { + {10, 3, "Cubic (p=3) with 10 control points"}, + {10, 4, "Quartic (p=4) with 10 control points"}, + {20, 3, "Cubic (p=3) with 20 control points"}, + }; + + for (size_t i = 0; i < test_cases.size(); ++i) { + const auto& tc = test_cases[i]; + ex::print_separator('='); + std::cout << "Test case " << (i + 1) << ": " << tc.title << "\n\n"; + + const ApproximationBSpline approx( + sample_points, tc.num_cps, tc.degree); + + const double error = approx.calculate_approximation_error(); + ex::print_value("Approximation error", error, 2); + ex::print_value("Num control points", static_cast(approx.n_control_points()), 0); + ex::print_value("Degree", static_cast(approx.degree()), 0); + + std::cout << "\n"; + ex::print_vector("Knot vector", approx.knots()); + ex::print_matrix("Control points", approx.control_points()); + + // Generate and print a few curve points + const auto [params, curve_pts] = approx.generate_curve_points(10); + std::cout << "\nSample curve points:\n"; + const int w = 14; + std::cout << std::right << std::setw(w) << "u" + << std::setw(w) << "X" << std::setw(w) << "Y" << "\n"; + ex::print_separator('-', 3 * w); + for (Eigen::Index j = 0; j < curve_pts.rows(); ++j) { + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << params(j) + << std::setw(w) << curve_pts(j, 0) + << std::setw(w) << curve_pts(j, 1) << "\n"; + } + } +} + +/// Demonstrate degree comparison on a heart-shaped curve. +static void example_degree_comparison() { + ex::print_header("Approximation -- Degree Comparison"); + + // Generate heart-shaped points + const int n = 100; + Eigen::MatrixXd heart_points(n, 2); + for (int i = 0; i < n; ++i) { + const double t = 2.0 * M_PI * static_cast(i) / n; + heart_points(i, 0) = 16.0 * std::pow(std::sin(t), 3) * 10.0 + 150.0; + heart_points(i, 1) = (13.0 * std::cos(t) - 5.0 * std::cos(2.0 * t) + - 2.0 * std::cos(3.0 * t) - std::cos(4.0 * t)) * 10.0 + 150.0; + } + + std::cout << "Heart shape: " << heart_points.rows() << " sample points\n\n"; + + const int num_cps = 12; + const std::vector degrees = {2, 3, 4}; + + for (const int degree : degrees) { + const ApproximationBSpline approx(heart_points, num_cps, degree); + const double error = approx.calculate_approximation_error(); + + std::cout << "Degree " << degree + << " (n_cp=" << num_cps << "): error = " + << std::fixed << std::setprecision(2) << error << "\n"; + } +} + +/// Demonstrate control-point-count comparison on a spiral. +static void example_cp_count_comparison() { + ex::print_header("Approximation -- Control Point Count Comparison"); + + // Generate spiral points + const int n = 100; + Eigen::MatrixXd spiral_points(n, 2); + for (int i = 0; i < n; ++i) { + const double t = 6.0 * M_PI * static_cast(i) / (n - 1); + const double r = 5.0 + 15.0 * t; + spiral_points(i, 0) = r * std::cos(t) + 150.0; + spiral_points(i, 1) = r * std::sin(t) + 150.0; + } + + std::cout << "Spiral: " << spiral_points.rows() << " sample points\n\n"; + + const int degree = 3; + const std::vector cp_counts = {8, 15, 25}; + + for (const int num_cps : cp_counts) { + const ApproximationBSpline approx(spiral_points, num_cps, degree); + const double error = approx.calculate_approximation_error(); + + std::cout << "CPs = " << std::setw(3) << num_cps + << " (degree=" << degree << "): error = " + << std::fixed << std::setprecision(2) << error << "\n"; + } +} + +/// Demonstrate weighted approximation. +static void example_weighted_approximation() { + ex::print_header("Approximation -- Weighted Fit"); + + // Circle with noise + const int n = 60; + Eigen::MatrixXd circle_points(n, 2); + for (int i = 0; i < n; ++i) { + const double t = 2.0 * M_PI * static_cast(i) / n; + circle_points(i, 0) = 100.0 * std::cos(t) + 150.0; + circle_points(i, 1) = 100.0 * std::sin(t) + 150.0; + } + + const int num_cps = 10; + const int degree = 3; + + // Uniform weights + const ApproximationBSpline approx_uniform(circle_points, num_cps, degree); + const double error_uniform = approx_uniform.calculate_approximation_error(); + + // Custom weights: emphasize first half of the points + Eigen::VectorXd weights = Eigen::VectorXd::Ones(n); + for (int i = 0; i < n / 2; ++i) { + weights(i) = 5.0; + } + + const ApproximationBSpline approx_weighted( + circle_points, num_cps, degree, weights); + const double error_weighted = approx_weighted.calculate_approximation_error(); + + std::cout << "Circle approximation (" << n << " points, " << num_cps << " CPs):\n"; + ex::print_value("Uniform weights error", error_uniform, 2); + ex::print_value("Weighted (first half emphasized) error", error_weighted, 2); +} + +// ============================================================================ +// Part 2: Smoothing Cubic B-spline +// ============================================================================ + +/// Example 8.12: Smoothing B-spline with different lambda values. +static void example_8_12() { + ex::print_header("Part 2 -- Smoothing Cubic B-spline (Example 8.12)"); + + // Points from the example + Eigen::MatrixXd points(6, 3); + points << 0, 0, 0, + 1, 2, 1, + 2, 3, 0, + 4, 3, 0, + 5, 2, 2, + 6, 0, 2; + + std::cout << "Approximation points:\n"; + ex::print_matrix("Points", points); + + // Test different lambda values + const std::vector lambda_values = {1e-4, 1e-5, 1e-6}; + + for (const double lambda_val : lambda_values) { + ex::print_separator('='); + + // Convert lambda to mu: lambda = (1 - mu) / (6 * mu) => mu = 1 / (6*lambda + 1) + const double mu = 1.0 / (6.0 * lambda_val + 1.0); + + std::cout << "Lambda = " << std::scientific << std::setprecision(6) << lambda_val + << ", Mu = " << std::fixed << std::setprecision(6) << mu << "\n\n"; + + BSplineParams params; + params.mu = mu; + params.method = Parameterization::ChordLength; + params.enforce_endpoints = true; + params.auto_derivatives = true; + + const SmoothingCubicBSpline spline(points, params); + + ex::print_value("Mu (stored)", spline.mu()); + ex::print_value("Lambda (stored)", spline.lambda_param()); + ex::print_value("Degree", static_cast(spline.degree()), 0); + ex::print_value("Num control points", static_cast(spline.n_control_points()), 0); + + std::cout << "\n"; + ex::print_matrix("Control points", spline.control_points()); + + // Approximation errors + const Eigen::VectorXd errors = spline.calculate_approximation_error(); + std::cout << "\n"; + ex::print_vector("Per-point errors", errors); + + const double total_error = spline.calculate_total_error(); + ex::print_value("Total error", total_error); + + // Evaluate along the curve + std::cout << "\nCurve samples:\n"; + const int w = 14; + std::cout << std::right + << std::setw(w) << "u" + << std::setw(w) << "X" + << std::setw(w) << "Y" + << std::setw(w) << "Z" << "\n"; + ex::print_separator('-', 4 * w); + + const int n_samples = 10; + const double u_start = spline.u_min(); + const double u_end = spline.u_max(); + for (int i = 0; i <= n_samples; ++i) { + const double u = u_start + (u_end - u_start) * static_cast(i) / n_samples; + const Eigen::VectorXd pt = spline.evaluate(u); + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << u + << std::setw(w) << pt(0) + << std::setw(w) << pt(1) + << std::setw(w) << pt(2) << "\n"; + } + } +} + +/// Demonstrate smoothing with different mu values on 2D data. +static void example_smoothing_mu_comparison() { + ex::print_header("Smoothing -- Mu Comparison (2D)"); + + // Simple 2D points + Eigen::MatrixXd points(8, 2); + points << 0, 0, + 1, 3, + 2, 1, + 3, 4, + 4, 2, + 5, 5, + 6, 1, + 7, 0; + + const std::vector mu_values = {0.1, 0.5, 0.9}; + + for (const double mu : mu_values) { + BSplineParams params; + params.mu = mu; + params.method = Parameterization::ChordLength; + params.enforce_endpoints = true; + params.auto_derivatives = true; + + const SmoothingCubicBSpline spline(points, params); + + const double total_error = spline.calculate_total_error(); + std::cout << "Mu = " << std::fixed << std::setprecision(2) << mu + << ": total_error = " << std::setprecision(6) << total_error + << ", n_cp = " << spline.n_control_points() << "\n"; + } +} + +/// Demonstrate smoothing with explicit endpoint derivatives. +static void example_smoothing_with_derivatives() { + ex::print_header("Smoothing -- Explicit Endpoint Derivatives"); + + Eigen::MatrixXd points(6, 3); + points << 0, 0, 0, + 1, 2, 1, + 2, 3, 0, + 4, 3, 0, + 5, 2, 2, + 6, 0, 2; + + Eigen::VectorXd v0(3), vn(3); + v0 << 4.43, 8.87, 4.43; + vn << 4.85, -9.71, 0.0; + + BSplineParams params; + params.mu = 0.999; + params.v0 = v0; + params.vn = vn; + params.method = Parameterization::ChordLength; + params.enforce_endpoints = true; + params.auto_derivatives = false; + + const SmoothingCubicBSpline spline(points, params); + + std::cout << "With explicit endpoint derivatives:\n"; + ex::print_vector("v0", v0); + ex::print_vector("vn", vn); + ex::print_value("Mu", spline.mu()); + ex::print_value("Total error", spline.calculate_total_error()); + std::cout << "\n"; + ex::print_matrix("Control points", spline.control_points()); + + // Second derivative magnitudes at a few points + ex::print_separator(); + std::cout << "Second derivative magnitude along curve:\n\n"; + const int n_samples = 8; + for (int i = 0; i <= n_samples; ++i) { + const double u = spline.u_min() + + (spline.u_max() - spline.u_min()) * static_cast(i) / n_samples; + const Eigen::VectorXd d2 = spline.evaluate_derivative(u, 2); + const double mag = d2.norm(); + std::cout << " u = " << std::fixed << std::setprecision(4) << u + << ": ||s''(u)|| = " << std::setprecision(4) << mag << "\n"; + } +} + +int main() { + // Part 1: Approximation + example_approximation(); + example_degree_comparison(); + example_cp_count_comparison(); + example_weighted_approximation(); + + // Part 2: Smoothing + example_8_12(); + example_smoothing_mu_comparison(); + example_smoothing_with_derivatives(); + + return 0; +} diff --git a/cpp/examples/bspline_cubic_example.cpp b/cpp/examples/bspline_cubic_example.cpp new file mode 100644 index 0000000..7a12cf0 --- /dev/null +++ b/cpp/examples/bspline_cubic_example.cpp @@ -0,0 +1,182 @@ +/// Cubic B-spline interpolation example -- C++ port of examples/b_spline_cubic_ex.py +/// +/// Demonstrates CubicBSplineInterpolation through 3D points (Example 8.8) +/// with chord-length parameterization and auto-derivative computation. + +#include +#include + +#include "example_utils.hpp" + +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::bspline; + +/// Example 8.8: 3D cubic B-spline interpolation. +static void example_8_8() { + ex::print_header("Example 8.8 -- 3D Cubic B-spline Interpolation"); + + // Define the interpolation points from the example + Eigen::MatrixXd points(10, 3); + points << 83, -54, 119, + -64, 10, 124, + 42, 79, 226, + -98, 23, 222, + -13, 125, 102, + 140, 81, 92, + 43, 32, 92, + -65, -17, 134, + -45, -89, 182, + 71, 90, 192; + + // Create cubic B-spline interpolation with chord-length parameterization + const CubicBSplineInterpolation interpolation( + points, + std::nullopt, // v0 (auto) + std::nullopt, // vn (auto) + Parameterization::ChordLength, + true // auto_derivatives + ); + + // Print basic info + std::cout << "Cubic B-spline Interpolation Information:\n"; + ex::print_value("Number of interpolation points", + static_cast(points.rows()), 0); + ex::print_value("Degree", static_cast(interpolation.degree()), 0); + ex::print_value("Dimension", static_cast(interpolation.dimension()), 0); + ex::print_value("u_min", interpolation.u_min()); + ex::print_value("u_max", interpolation.u_max()); + + // Print parameter values (u-bars) + std::cout << "\n"; + ex::print_vector("Parameter values (u_bars)", interpolation.u_bars()); + + // Print the knot vector + ex::print_vector("Knot vector", interpolation.knots()); + + // Print start/end derivatives + std::cout << "\n"; + ex::print_vector("Start derivative (v0)", interpolation.start_derivative()); + ex::print_vector("End derivative (vn)", interpolation.end_derivative()); + + // Print control points + std::cout << "\n"; + ex::print_matrix("Control points", interpolation.control_points()); + + // Print interpolation points + std::cout << "\n"; + ex::print_matrix("Interpolation points", interpolation.interpolation_points()); + + // Verify interpolation: evaluate at each parameter value + ex::print_separator('='); + std::cout << "Interpolation verification (evaluate at each u_bar):\n\n"; + + const int w = 12; + std::cout << std::right + << std::setw(8) << "u_bar" + << std::setw(w) << "X_eval" + << std::setw(w) << "Y_eval" + << std::setw(w) << "Z_eval" + << std::setw(w) << "X_orig" + << std::setw(w) << "Y_orig" + << std::setw(w) << "Z_orig" << "\n"; + ex::print_separator('-', 8 + 6 * w); + + for (Eigen::Index i = 0; i < points.rows(); ++i) { + const double u = interpolation.u_bars()(i); + const Eigen::VectorXd evaluated = interpolation.evaluate(u); + + std::cout << std::fixed << std::setprecision(4) + << std::setw(8) << u + << std::setw(w) << evaluated(0) + << std::setw(w) << evaluated(1) + << std::setw(w) << evaluated(2) + << std::setw(w) << points(i, 0) + << std::setw(w) << points(i, 1) + << std::setw(w) << points(i, 2) << "\n"; + } + + // Evaluate derivatives at a few points along the curve + ex::print_separator('='); + std::cout << "Derivative evaluation at sample parameter values:\n\n"; + + const int n_samples = 5; + const double u_start = interpolation.u_min(); + const double u_end = interpolation.u_max(); + + for (int i = 0; i <= n_samples; ++i) { + const double u = u_start + (u_end - u_start) * static_cast(i) / n_samples; + const Eigen::VectorXd pos = interpolation.evaluate(u); + const Eigen::VectorXd vel = interpolation.evaluate_derivative(u, 1); + const Eigen::VectorXd acc = interpolation.evaluate_derivative(u, 2); + + std::cout << "u = " << std::fixed << std::setprecision(4) << u << ":\n"; + ex::print_vector(" Position", pos); + ex::print_vector(" Velocity", vel); + ex::print_vector(" Acceleration", acc); + std::cout << "\n"; + } +} + +/// Demonstrate with explicit endpoint derivatives. +static void example_with_derivatives() { + ex::print_header("Cubic B-spline with Explicit Derivatives"); + + // Simple 2D points + Eigen::MatrixXd points(5, 2); + points << 0, 0, + 1, 2, + 3, 3, + 5, 1, + 6, 0; + + // Specify endpoint derivatives + Eigen::VectorXd v0(2); + v0 << 1.0, 3.0; + + Eigen::VectorXd vn(2); + vn << 1.0, -2.0; + + const CubicBSplineInterpolation interpolation( + points, v0, vn, + Parameterization::ChordLength, + false // not auto_derivatives + ); + + std::cout << "Interpolation with explicit derivatives:\n"; + ex::print_value("Number of points", static_cast(points.rows()), 0); + ex::print_value("Degree", static_cast(interpolation.degree()), 0); + ex::print_vector("Start derivative (v0)", interpolation.start_derivative()); + ex::print_vector("End derivative (vn)", interpolation.end_derivative()); + + std::cout << "\n"; + ex::print_vector("u_bars", interpolation.u_bars()); + ex::print_vector("Knots", interpolation.knots()); + + std::cout << "\n"; + ex::print_matrix("Control points", interpolation.control_points()); + + // Generate curve points and print a summary + const auto [params, curve_pts] = interpolation.generate_curve_points(15); + std::cout << "\nCurve samples:\n"; + + const int w = 14; + std::cout << std::right << std::setw(w) << "u" + << std::setw(w) << "X" << std::setw(w) << "Y" << "\n"; + ex::print_separator('-', 3 * w); + + for (Eigen::Index i = 0; i < curve_pts.rows(); ++i) { + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << params(i) + << std::setw(w) << curve_pts(i, 0) + << std::setw(w) << curve_pts(i, 1) << "\n"; + } +} + +int main() { + example_8_8(); + example_with_derivatives(); + + return 0; +} diff --git a/cpp/examples/bspline_example.cpp b/cpp/examples/bspline_example.cpp new file mode 100644 index 0000000..0d855c0 --- /dev/null +++ b/cpp/examples/bspline_example.cpp @@ -0,0 +1,229 @@ +/// B-spline example -- C++ port of examples/b_spline_ex.py +/// +/// Demonstrates BSpline construction, basis function evaluation, +/// derivative computation, curve point generation, and 3D curves. + +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::bspline; + +/// Create the 2D B-spline from the document example. +static BSpline create_example_bspline() { + const int degree = 3; + + Eigen::MatrixXd control_points(7, 2); + control_points << 1, 2, + 2, 3, + 3, -3, + 4, 4, + 5, 5, + 6, -5, + 7, -6; + + const std::vector knots = {0, 0, 0, 0, 1, 2, 4, 7, 7, 7, 7}; + + return BSpline(degree, std::span(knots), control_points); +} + +/// Demonstrate basic B-spline evaluation and basis functions. +static void demonstrate_basic_bspline() { + ex::print_header("Basic B-spline Evaluation"); + + const auto bspline = create_example_bspline(); + + std::cout << "B-spline properties:\n"; + ex::print_value("Degree", static_cast(bspline.degree()), 0); + ex::print_value("Number of control points", static_cast(bspline.n_control_points()), 0); + ex::print_value("Dimension", static_cast(bspline.dimension()), 0); + ex::print_value("u_min", bspline.u_min()); + ex::print_value("u_max", bspline.u_max()); + + // Evaluate at u = 1.5 + const double u_value = 1.5; + const Eigen::VectorXd point = bspline.evaluate(u_value); + + std::cout << "\nPoint at u = " << u_value << ":\n"; + ex::print_vector("Position", point); + + // Basis functions at u = 1.5 + const int span = bspline.find_knot_span(u_value); + const Eigen::VectorXd basis = bspline.basis_functions(u_value, span); + + std::cout << "\nKnot span index at u = " << u_value << ": " << span << "\n"; + std::cout << "Non-zero basis functions:\n"; + for (Eigen::Index i = 0; i < basis.size(); ++i) { + std::cout << " B^" << bspline.degree() << "_" + << (span - bspline.degree() + static_cast(i)) + << " = " << std::fixed << std::setprecision(4) << basis(i) << "\n"; + } + + // Print knot vector + std::cout << "\n"; + ex::print_vector("Knot vector", bspline.knots()); + ex::print_matrix("Control points", bspline.control_points()); +} + +/// Example B.6: Basis function derivatives at u = 4.5. +static void example_b6() { + ex::print_header("Example B.6 -- Basis Function Derivatives"); + + const int degree = 3; + const std::vector knots = {0, 0, 0, 0, 1, 2, 4, 7, 7, 7, 7}; + + // Dummy control points (basis functions don't depend on them) + Eigen::MatrixXd control_points = Eigen::MatrixXd::Zero(7, 2); + + const BSpline bspline(degree, std::span(knots), control_points); + + const double u_value = 4.5; + const int span = bspline.find_knot_span(u_value); + std::cout << "For u = " << u_value << ", the knot span index is: " << span << "\n"; + + // Calculate derivatives up to order 3 + const Eigen::MatrixXd derivatives = bspline.basis_function_derivatives(u_value, span, 3); + + std::cout << "\nBasis function values and derivatives at u = 4.5:\n"; + ex::print_separator('-', 80); + + for (int k = 0; k < 4; ++k) { + std::cout << "Ders[" << k << "]: "; + for (int j = 0; j < 4; ++j) { + if (j > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << derivatives(k, j); + } + std::cout << "\n"; + } + + std::cout << "\nWhich correspond to:\n"; + ex::print_separator('-', 80); + + const std::vector labels = {"B_i^3 ", "B_i^3(1) ", "B_i^3(2) ", "B_i^3(3) "}; + for (int k = 0; k < 4; ++k) { + std::cout << labels[static_cast(k)] << ": "; + for (int j = 0; j < 4; ++j) { + const int idx = span - degree + j; + if (j > 0) std::cout << ", "; + std::cout << "B_" << idx << " = " << std::fixed << std::setprecision(4) + << derivatives(k, j); + } + std::cout << "\n"; + } + + std::cout << "\nAll the other terms B_j^3(k) are null.\n"; +} + +/// Demonstrate curve point generation. +static void demonstrate_curve_generation() { + ex::print_header("Curve Point Generation"); + + const auto bspline = create_example_bspline(); + + // Generate curve points + const int num_points = 20; + const auto [params, curve_points] = bspline.generate_curve_points(num_points); + + std::cout << "Generated " << curve_points.rows() << " curve points:\n\n"; + + const int w = 14; + std::cout << std::right << std::setw(w) << "u" + << std::setw(w) << "X" << std::setw(w) << "Y" << "\n"; + ex::print_separator('-', 3 * w); + + for (Eigen::Index i = 0; i < curve_points.rows(); ++i) { + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << params(i) + << std::setw(w) << curve_points(i, 0) + << std::setw(w) << curve_points(i, 1) << "\n"; + } +} + +/// Demonstrate 3D B-spline with uniform knots. +static void demonstrate_3d_bspline() { + ex::print_header("3D B-spline Curve"); + + const int degree = 3; + + Eigen::MatrixXd control_points(6, 3); + control_points << 0, 0, 0, + 1, 1, 2, + 2, -1, 1, + 3, 0, 3, + 4, 2, 0, + 5, 0, 1; + + const Eigen::VectorXd knots = BSpline::create_uniform_knots(degree, 6); + + const BSpline bspline(degree, std::span(knots.data(), static_cast(knots.size())), + control_points); + + std::cout << "B-spline properties:\n"; + ex::print_value("Degree", static_cast(bspline.degree()), 0); + ex::print_value("Number of control points", static_cast(bspline.n_control_points()), 0); + ex::print_value("Dimension", static_cast(bspline.dimension()), 0); + ex::print_value("u_min", bspline.u_min()); + ex::print_value("u_max", bspline.u_max()); + + std::cout << "\n"; + ex::print_vector("Uniform knot vector", bspline.knots()); + ex::print_matrix("Control points (3D)", bspline.control_points()); + + // Evaluate at midpoint + const double u_mid = (bspline.u_min() + bspline.u_max()) / 2.0; + const Eigen::VectorXd mid_point = bspline.evaluate(u_mid); + std::cout << "\nPoint at u_mid = " << std::fixed << std::setprecision(4) << u_mid << ":\n"; + ex::print_vector("Position", mid_point); + + // First derivative at midpoint + const Eigen::VectorXd deriv1 = bspline.evaluate_derivative(u_mid, 1); + ex::print_vector("1st derivative", deriv1); + + // Second derivative at midpoint + const Eigen::VectorXd deriv2 = bspline.evaluate_derivative(u_mid, 2); + ex::print_vector("2nd derivative", deriv2); + + // Generate 3D curve points + const auto [params, curve_pts] = bspline.generate_curve_points(10); + std::cout << "\nSample 3D curve points:\n"; + const int w = 14; + std::cout << std::right << std::setw(w) << "u" + << std::setw(w) << "X" << std::setw(w) << "Y" << std::setw(w) << "Z" << "\n"; + ex::print_separator('-', 4 * w); + for (Eigen::Index i = 0; i < curve_pts.rows(); ++i) { + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << params(i) + << std::setw(w) << curve_pts(i, 0) + << std::setw(w) << curve_pts(i, 1) + << std::setw(w) << curve_pts(i, 2) << "\n"; + } +} + +/// Demonstrate periodic knot generation. +static void demonstrate_periodic_knots() { + ex::print_header("Periodic Knots"); + + const int degree = 3; + const int n_cp = 6; + + const Eigen::VectorXd uniform_knots = BSpline::create_uniform_knots(degree, n_cp, 0.0, 1.0); + const Eigen::VectorXd periodic_knots = BSpline::create_periodic_knots(degree, n_cp, 0.0, 1.0); + + ex::print_vector("Uniform knots (p=3, n_cp=6)", uniform_knots); + ex::print_vector("Periodic knots (p=3, n_cp=6)", periodic_knots); +} + +int main() { + demonstrate_basic_bspline(); + example_b6(); + demonstrate_curve_generation(); + demonstrate_3d_bspline(); + demonstrate_periodic_knots(); + + return 0; +} diff --git a/cpp/examples/bspline_interpolator_example.cpp b/cpp/examples/bspline_interpolator_example.cpp new file mode 100644 index 0000000..fdf091f --- /dev/null +++ b/cpp/examples/bspline_interpolator_example.cpp @@ -0,0 +1,360 @@ +/// B-spline interpolator example -- C++ port of examples/b_spline_interpolate_ex.py +/// +/// Demonstrates BSplineInterpolator with multiple configurations: +/// cubic with velocity constraints, degree 4 (jerk-continuous), +/// degree 5 with acceleration constraints, cyclic, and 3D interpolation. + +#include + +#include "example_utils.hpp" + +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::bspline; + +/// Helper: print a scalar trajectory table for BSplineInterpolator. +static void print_scalar_trajectory(const BSplineInterpolator& interp, + double t_start, double t_end, + int num_samples = 15) { + ex::print_trajectory_table( + [&](double t) { return interp.evaluate(t)(0); }, + [&](double t) { return interp.evaluate_derivative(t, 1)(0); }, + [&](double t) { return interp.evaluate_derivative(t, 2)(0); }, + t_start, t_end, num_samples); +} + +/// Example 1: Cubic B-spline with velocity constraints (Example 4.16). +static void example_cubic_bspline() { + ex::print_header("Example 1 -- Cubic B-spline with Velocity Constraints"); + + Eigen::VectorXd times(7); + times << 0, 5, 7, 8, 10, 15, 18; + + Eigen::MatrixXd points(7, 1); + points << 3, -2, -5, 0, 6, 12, 8; + + Eigen::VectorXd v0(1); + v0 << 2.0; + Eigen::VectorXd vn(1); + vn << -3.0; + + const BSplineInterpolator interp( + 3, // degree + points, + times, + v0, // initial_velocity + vn // final_velocity + ); + + std::cout << "Properties:\n"; + ex::print_value("Degree", static_cast(interp.degree()), 0); + ex::print_value("Num control points", static_cast(interp.n_control_points()), 0); + ex::print_value("u_min", interp.u_min()); + ex::print_value("u_max", interp.u_max()); + + std::cout << "\n"; + ex::print_vector("Times", interp.times()); + ex::print_vector("Knots", interp.knots()); + + std::cout << "\nTrajectory evaluation:\n\n"; + print_scalar_trajectory(interp, times(0), times(times.size() - 1)); + + // Verify boundary velocities + ex::print_separator('='); + std::cout << "Boundary verification:\n"; + ex::print_value("Velocity at t_start", interp.evaluate_derivative(times(0), 1)(0)); + ex::print_value("Expected v0", 2.0); + ex::print_value("Velocity at t_end", interp.evaluate_derivative(times(times.size() - 1), 1)(0)); + ex::print_value("Expected vn", -3.0); +} + +/// Example 2: Degree 4 B-spline (jerk-continuous). +static void example_jerk_continuous() { + ex::print_header("Example 2 -- Degree 4 B-spline (Jerk Continuous)"); + + Eigen::VectorXd times(7); + times << 0, 5, 7, 8, 10, 15, 18; + + Eigen::MatrixXd points(7, 1); + points << 3, -2, -5, 0, 6, 12, 8; + + Eigen::VectorXd v0(1), vn(1), a0(1), an(1); + v0 << 2.0; + vn << -3.0; + a0 << 0.0; + an << 0.0; + + const BSplineInterpolator interp( + 4, // degree + points, + times, + v0, // initial_velocity + vn, // final_velocity + a0, // initial_acceleration + an // final_acceleration + ); + + std::cout << "Properties:\n"; + ex::print_value("Degree", static_cast(interp.degree()), 0); + ex::print_value("Num control points", static_cast(interp.n_control_points()), 0); + + std::cout << "\n"; + ex::print_vector("Knots", interp.knots()); + + // Print trajectory with jerk + std::cout << "\nTrajectory evaluation (with jerk):\n\n"; + const double t_start = times(0); + const double t_end = times(times.size() - 1); + const int num_samples = 15; + + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + return {interp.evaluate(t)(0), + interp.evaluate_derivative(t, 1)(0), + interp.evaluate_derivative(t, 2)(0), + interp.evaluate_derivative(t, 3)(0)}; + }, + t_start, t_end, num_samples); + + // Verify boundary conditions + ex::print_separator('='); + std::cout << "Boundary verification:\n"; + ex::print_value("Velocity at t_start", interp.evaluate_derivative(t_start, 1)(0)); + ex::print_value("Acceleration at t_start", interp.evaluate_derivative(t_start, 2)(0)); + ex::print_value("Velocity at t_end", interp.evaluate_derivative(t_end, 1)(0)); + ex::print_value("Acceleration at t_end", interp.evaluate_derivative(t_end, 2)(0)); +} + +/// Example 3: Degree 5 B-spline with acceleration constraints. +static void example_degree5() { + ex::print_header("Example 3 -- Degree 5 B-spline with Acceleration Constraints"); + + Eigen::VectorXd times(7); + times << 0, 1, 2, 3, 4, 5, 6; + + Eigen::MatrixXd points(7, 1); + points << 0, 2, 1, 3, 2, 4, 3; + + Eigen::VectorXd v0(1), vn(1), a0(1), an(1); + v0 << 1.0; + vn << -1.0; + a0 << 0.0; + an << 0.0; + + const BSplineInterpolator interp( + 5, // degree + points, + times, + v0, vn, + a0, an + ); + + std::cout << "Properties:\n"; + ex::print_value("Degree", static_cast(interp.degree()), 0); + ex::print_value("Num control points", static_cast(interp.n_control_points()), 0); + + std::cout << "\n"; + ex::print_vector("Knots", interp.knots()); + + std::cout << "\nTrajectory evaluation:\n\n"; + print_scalar_trajectory(interp, times(0), times(times.size() - 1)); + + // Verify boundary conditions + ex::print_separator('='); + std::cout << "Boundary verification:\n"; + const double t_start = times(0); + const double t_end = times(times.size() - 1); + ex::print_value("Velocity at t_start", interp.evaluate_derivative(t_start, 1)(0)); + ex::print_value("Acceleration at t_start", interp.evaluate_derivative(t_start, 2)(0)); + ex::print_value("Velocity at t_end", interp.evaluate_derivative(t_end, 1)(0)); + ex::print_value("Acceleration at t_end", interp.evaluate_derivative(t_end, 2)(0)); +} + +/// Example 4: Cyclic B-spline (Example 4.17). +static void example_cyclic() { + ex::print_header("Example 4 -- Cyclic B-spline (Degree 4)"); + + Eigen::VectorXd times(7); + times << 0, 5, 7, 8, 10, 15, 18; + + // Last point equals first point for cyclic + Eigen::MatrixXd points(7, 1); + points << 3, -2, -5, 0, 6, 12, 3; + + const BSplineInterpolator interp( + 4, // degree + points, + times, + std::nullopt, // initial_velocity + std::nullopt, // final_velocity + std::nullopt, // initial_acceleration + std::nullopt, // final_acceleration + true // cyclic + ); + + std::cout << "Properties:\n"; + ex::print_value("Degree", static_cast(interp.degree()), 0); + ex::print_value("Num control points", static_cast(interp.n_control_points()), 0); + ex::print_value("Cyclic", 1.0, 0); + + std::cout << "\n"; + ex::print_vector("Knots", interp.knots()); + + // Trajectory with jerk and snap + std::cout << "\nTrajectory evaluation:\n\n"; + const double t_start = times(0); + const double t_end = times(times.size() - 1); + + const int w = 14; + const int p = 6; + std::cout << std::right + << std::setw(w) << "Time" + << std::setw(w) << "Position" + << std::setw(w) << "Velocity" + << std::setw(w) << "Acceleration" + << std::setw(w) << "Jerk" + << std::setw(w) << "Snap" << "\n"; + ex::print_separator('-', 6 * w); + + const int num_samples = 15; + for (int i = 0; i <= num_samples; ++i) { + const double t = t_start + (t_end - t_start) * static_cast(i) / num_samples; + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << t + << std::setw(w) << interp.evaluate(t)(0) + << std::setw(w) << interp.evaluate_derivative(t, 1)(0) + << std::setw(w) << interp.evaluate_derivative(t, 2)(0) + << std::setw(w) << interp.evaluate_derivative(t, 3)(0) + << std::setw(w) << interp.evaluate_derivative(t, 4)(0) + << "\n"; + } + std::cout << "\n"; + + // Verify cyclic continuity: values at start and end should match + ex::print_separator('='); + std::cout << "Cyclic continuity verification:\n"; + ex::print_value("Position at t_start", interp.evaluate(t_start)(0)); + ex::print_value("Position at t_end", interp.evaluate(t_end)(0)); + ex::print_value("Velocity at t_start", interp.evaluate_derivative(t_start, 1)(0)); + ex::print_value("Velocity at t_end", interp.evaluate_derivative(t_end, 1)(0)); + ex::print_value("Acceleration at t_start", interp.evaluate_derivative(t_start, 2)(0)); + ex::print_value("Acceleration at t_end", interp.evaluate_derivative(t_end, 2)(0)); +} + +/// Example 5: 3D B-spline interpolation. +static void example_3d() { + ex::print_header("Example 5 -- 3D B-spline Interpolation"); + + Eigen::VectorXd times(5); + times << 0, 1, 2, 3, 4; + + Eigen::MatrixXd points(5, 3); + points << 0, 0, 0, + 1, 1, 2, + 2, 0, 3, + 3, -1, 2, + 4, 0, 0; + + const int degree = 3; + const BSplineInterpolator interp(degree, points, times); + + std::cout << "Properties:\n"; + ex::print_value("Degree", static_cast(interp.degree()), 0); + ex::print_value("Num control points", static_cast(interp.n_control_points()), 0); + ex::print_value("Dimension", static_cast(interp.dimension()), 0); + std::cout << "Continuity: C^" << (degree - 1) << " (continuous acceleration)\n"; + + std::cout << "\n"; + ex::print_vector("Knots", interp.knots()); + ex::print_matrix("Control points (3D)", interp.control_points()); + + // Print original and interpolated points + std::cout << "\nOriginal points to interpolate:\n"; + for (Eigen::Index i = 0; i < points.rows(); ++i) { + std::cout << " Point " << i << " (t=" << times(i) << "): (" + << std::fixed << std::setprecision(1) + << points(i, 0) << ", " << points(i, 1) << ", " << points(i, 2) << ")\n"; + } + + // Evaluate at intermediate times + std::cout << "\nInterpolated points at intermediate times:\n"; + const std::vector t_samples = {0.5, 1.5, 2.5, 3.5}; + for (const double t : t_samples) { + const Eigen::VectorXd pt = interp.evaluate(t); + std::cout << " t = " << std::fixed << std::setprecision(1) << t << ": (" + << std::setprecision(3) << pt(0) << ", " << pt(1) << ", " << pt(2) << ")\n"; + } + + // Print full 3D trajectory + std::cout << "\n3D trajectory table:\n\n"; + const int w = 14; + std::cout << std::right + << std::setw(w) << "Time" + << std::setw(w) << "X" << std::setw(w) << "Y" << std::setw(w) << "Z" << "\n"; + ex::print_separator('-', 4 * w); + + const int num_samples = 12; + for (int i = 0; i <= num_samples; ++i) { + const double t = times(0) + (times(times.size() - 1) - times(0)) + * static_cast(i) / num_samples; + const Eigen::VectorXd pt = interp.evaluate(t); + std::cout << std::fixed << std::setprecision(4) + << std::setw(w) << t + << std::setw(w) << pt(0) + << std::setw(w) << pt(1) + << std::setw(w) << pt(2) << "\n"; + } + + // Try higher degrees with more points + ex::print_separator('='); + std::cout << "Degree sensitivity with 5 points:\n\n"; + + // Degree 3 works (already demonstrated). Try degree 4 and 5. + for (const int deg : {4, 5}) { + std::cout << " Trying degree " << deg << " with " << points.rows() << " points... "; + try { + const BSplineInterpolator test_interp(deg, points, times); + std::cout << "Succeeded! (n_cp=" << test_interp.n_control_points() << ")\n"; + } catch (const std::exception& e) { + std::cout << "Failed: " << e.what() << "\n"; + } + } + + // Extended points for higher degrees + ex::print_separator(); + std::cout << "Extended points (7 points) for higher degrees:\n\n"; + + Eigen::VectorXd times_ext(7); + times_ext << 0, 1, 2, 3, 4, 5, 6; + + Eigen::MatrixXd points_ext(7, 3); + points_ext << 0, 0, 0, + 1, 1, 2, + 2, 0, 3, + 3, -1, 2, + 4, 0, 0, + 5, 1, -1, + 6, 0, -2; + + for (const int deg : {3, 4, 5}) { + std::cout << " Degree " << deg << " with 7 points: "; + try { + const BSplineInterpolator ext_interp(deg, points_ext, times_ext); + std::cout << "Succeeded! (n_cp=" << ext_interp.n_control_points() << ")\n"; + } catch (const std::exception& e) { + std::cout << "Failed: " << e.what() << "\n"; + } + } +} + +int main() { + example_cubic_bspline(); + example_jerk_continuous(); + example_degree5(); + example_cyclic(); + example_3d(); + + return 0; +} diff --git a/cpp/examples/concepts_example.cpp b/cpp/examples/concepts_example.cpp new file mode 100644 index 0000000..abde367 --- /dev/null +++ b/cpp/examples/concepts_example.cpp @@ -0,0 +1,398 @@ +/// C++20 concepts example -- C++ port of examples/protocols_ex.py +/// +/// Demonstrates how C++20 concepts serve as the compile-time equivalent of +/// Python protocols (PEP 544). Template functions constrained by each concept +/// accept any concrete type that satisfies the required interface, with no +/// inheritance needed. + +#include + +// Concrete types for ScalarTrajectory +#include +#include + +// Concrete types for CurveEvaluator +#include +#include + +// Concrete types for GeometricPath +#include +#include + +// Concrete types for QuaternionTrajectory +#include +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; + +// --------------------------------------------------------------------------- +// 1. ScalarTrajectory -- generic sampling of position, velocity, acceleration +// --------------------------------------------------------------------------- + +template +void sample_scalar(const T& traj, double t_start, double t_end, int n, + const std::string& label) { + std::cout << " " << label << ":\n"; + + const int w = 14; + const int p = 6; + + std::cout << std::right + << std::setw(w) << "t" + << std::setw(w) << "Position" + << std::setw(w) << "Velocity" + << std::setw(w) << "Acceleration" + << "\n"; + ex::print_separator('-', 4 * w); + + for (int i = 0; i <= n; ++i) { + const double t = t_start + (t_end - t_start) * static_cast(i) / n; + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << t + << std::setw(w) << traj.evaluate(t) + << std::setw(w) << traj.evaluate_velocity(t) + << std::setw(w) << traj.evaluate_acceleration(t) + << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +// 2. CurveEvaluator -- generic sampling of parametric curve and derivatives +// --------------------------------------------------------------------------- + +template +void sample_curve(const T& curve, double u_start, double u_end, int n, + const std::string& label) { + std::cout << " " << label << ":\n"; + + const int w = 14; + const int p = 6; + + // Determine dimension from the first evaluation + const Eigen::VectorXd first_pt = curve.evaluate(u_start); + const int dim = static_cast(first_pt.size()); + + // Print header + std::cout << std::right << std::setw(w) << "u"; + for (int d = 0; d < dim; ++d) { + std::cout << std::setw(w) << ("P[" + std::to_string(d) + "]"); + } + for (int d = 0; d < dim; ++d) { + std::cout << std::setw(w) << ("D[" + std::to_string(d) + "]"); + } + std::cout << "\n"; + ex::print_separator('-', (1 + 2 * dim) * w); + + for (int i = 0; i <= n; ++i) { + const double u = u_start + (u_end - u_start) * static_cast(i) / n; + const Eigen::VectorXd pt = curve.evaluate(u); + const Eigen::VectorXd deriv = curve.evaluate_derivative(u, 1); + + std::cout << std::fixed << std::setprecision(p) << std::setw(w) << u; + for (int d = 0; d < dim; ++d) { + std::cout << std::setw(w) << pt(d); + } + for (int d = 0; d < dim; ++d) { + std::cout << std::setw(w) << deriv(d); + } + std::cout << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +// 3. GeometricPath -- generic 3D path sampling +// --------------------------------------------------------------------------- + +template +void sample_path(const T& path, double s_start, double s_end, int n, + const std::string& label) { + std::cout << " " << label << ":\n"; + + const int w = 12; + const int p = 4; + + std::cout << std::right + << std::setw(w) << "s" + << std::setw(w) << "Pos X" + << std::setw(w) << "Pos Y" + << std::setw(w) << "Pos Z" + << std::setw(w) << "Vel X" + << std::setw(w) << "Vel Y" + << std::setw(w) << "Vel Z" + << std::setw(w) << "Acc X" + << std::setw(w) << "Acc Y" + << std::setw(w) << "Acc Z" + << "\n"; + ex::print_separator('-', 10 * w); + + for (int i = 0; i <= n; ++i) { + const double s = s_start + (s_end - s_start) * static_cast(i) / n; + const Eigen::Vector3d pos = path.position(s); + const Eigen::Vector3d vel = path.velocity(s); + const Eigen::Vector3d acc = path.acceleration(s); + + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << s + << std::setw(w) << pos.x() + << std::setw(w) << pos.y() + << std::setw(w) << pos.z() + << std::setw(w) << vel.x() + << std::setw(w) << vel.y() + << std::setw(w) << vel.z() + << std::setw(w) << acc.x() + << std::setw(w) << acc.y() + << std::setw(w) << acc.z() + << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +// 4. QuaternionTrajectory -- generic quaternion sampling with angular velocity +// --------------------------------------------------------------------------- + +template +void sample_quaternion(const T& traj, double t_start, double t_end, int n, + const std::string& label) { + std::cout << " " << label << ":\n"; + + const int w = 12; + const int p = 4; + + std::cout << std::right + << std::setw(w) << "t" + << std::setw(w) << "q.w" + << std::setw(w) << "q.x" + << std::setw(w) << "q.y" + << std::setw(w) << "q.z" + << std::setw(w) << "omega.x" + << std::setw(w) << "omega.y" + << std::setw(w) << "omega.z" + << std::setw(w) << "|omega|" + << "\n"; + ex::print_separator('-', 9 * w); + + for (int i = 0; i <= n; ++i) { + const double t = t_start + (t_end - t_start) * static_cast(i) / n; + const Eigen::Quaterniond q = traj.evaluate(t); + const Eigen::Vector3d omega = traj.evaluate_velocity(t); + + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << t + << std::setw(w) << q.w() + << std::setw(w) << q.x() + << std::setw(w) << q.y() + << std::setw(w) << q.z() + << std::setw(w) << omega.x() + << std::setw(w) << omega.y() + << std::setw(w) << omega.z() + << std::setw(w) << omega.norm() + << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +// Compile-time concept verification helpers +// --------------------------------------------------------------------------- + +static void print_concept_conformance() { + ex::print_header("Concept Conformance Summary"); + + // ScalarTrajectory + std::cout << "ScalarTrajectory concept:\n"; + std::cout << " CubicSpline : " + << (interpolatecpp::ScalarTrajectory + ? "YES" : "NO") << "\n"; + std::cout << " CubicSmoothingSpline : " + << (interpolatecpp::ScalarTrajectory + ? "YES" : "NO") << "\n"; + std::cout << "\n"; + + // CurveEvaluator + std::cout << "CurveEvaluator concept:\n"; + std::cout << " BSpline : " + << (interpolatecpp::CurveEvaluator + ? "YES" : "NO") << "\n"; + std::cout << " BSplineInterpolator : " + << (interpolatecpp::CurveEvaluator + ? "YES" : "NO") << "\n"; + std::cout << "\n"; + + // GeometricPath + std::cout << "GeometricPath concept:\n"; + std::cout << " LinearPath : " + << (interpolatecpp::GeometricPath + ? "YES" : "NO") << "\n"; + std::cout << " CircularPath : " + << (interpolatecpp::GeometricPath + ? "YES" : "NO") << "\n"; + std::cout << "\n"; + + // QuaternionTrajectory + std::cout << "QuaternionTrajectory concept:\n"; + std::cout << " QuaternionSpline : " + << (interpolatecpp::QuaternionTrajectory + ? "YES" : "NO") << "\n"; + std::cout << " SquadC2 : " + << (interpolatecpp::QuaternionTrajectory + ? "YES" : "NO") << "\n"; + std::cout << " LogQuaternionInterp : " + << (interpolatecpp::QuaternionTrajectory + ? "YES" : "NO") << "\n"; +} + +// --------------------------------------------------------------------------- +// Example: ScalarTrajectory with CubicSpline and CubicSmoothingSpline +// --------------------------------------------------------------------------- + +static void example_scalar_trajectory() { + ex::print_header("1. ScalarTrajectory Concept"); + + std::cout << "The same template function 'sample_scalar' works with any type\n" + << "that satisfies the ScalarTrajectory concept.\n\n"; + + // CubicSpline + const std::vector t_points = {0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector q_points = {0.0, 5.0, 3.0, 8.0, 10.0}; + const interpolatecpp::spline::CubicSpline spline(t_points, q_points); + sample_scalar(spline, 0.0, 4.0, 10, "CubicSpline"); + + // CubicSmoothingSpline + const interpolatecpp::spline::CubicSmoothingSpline smooth(t_points, q_points, 0.8); + sample_scalar(smooth, 0.0, 4.0, 10, "CubicSmoothingSpline (mu=0.8)"); +} + +// --------------------------------------------------------------------------- +// Example: CurveEvaluator with BSpline and BSplineInterpolator +// --------------------------------------------------------------------------- + +static void example_curve_evaluator() { + ex::print_header("2. CurveEvaluator Concept"); + + std::cout << "The same template function 'sample_curve' works with any type\n" + << "that satisfies the CurveEvaluator concept.\n\n"; + + // BSplineInterpolator (interpolates through points) + Eigen::VectorXd times(5); + times << 0.0, 1.0, 2.0, 3.0, 4.0; + + Eigen::MatrixXd points(5, 2); + points << 0.0, 0.0, + 1.0, 2.0, + 3.0, 3.0, + 5.0, 1.0, + 6.0, 4.0; + + const interpolatecpp::bspline::BSplineInterpolator interpolator(3, points, times); + sample_curve(interpolator, interpolator.u_min(), interpolator.u_max(), 10, + "BSplineInterpolator (degree 3, 2D)"); + + // BSpline (approximation with explicit control polygon) + Eigen::MatrixXd ctrl_pts(6, 2); + ctrl_pts << 0.0, 0.0, + 1.0, 3.0, + 2.5, 4.0, + 4.0, 2.0, + 5.5, 3.5, + 6.0, 1.0; + + const int degree = 3; + const Eigen::VectorXd knots = interpolatecpp::bspline::BSpline::create_uniform_knots( + degree, static_cast(ctrl_pts.rows())); + + const interpolatecpp::bspline::BSpline bspline(degree, std::vector( + knots.data(), knots.data() + knots.size()), ctrl_pts); + sample_curve(bspline, bspline.u_min(), bspline.u_max(), 10, + "BSpline (degree 3, 6 control points, 2D)"); +} + +// --------------------------------------------------------------------------- +// Example: GeometricPath with LinearPath and CircularPath +// --------------------------------------------------------------------------- + +static void example_geometric_path() { + ex::print_header("3. GeometricPath Concept"); + + std::cout << "The same template function 'sample_path' works with any type\n" + << "that satisfies the GeometricPath concept.\n\n"; + + // LinearPath + const Eigen::Vector3d start(0.0, 0.0, 0.0); + const Eigen::Vector3d end(5.0, 3.0, 2.0); + const interpolatecpp::path::LinearPath linear(start, end); + sample_path(linear, 0.0, linear.length(), 8, "LinearPath (0,0,0) -> (5,3,2)"); + + // CircularPath + const Eigen::Vector3d axis(0.0, 0.0, 1.0); + const Eigen::Vector3d center(0.0, 0.0, 0.0); + const Eigen::Vector3d circle_pt(3.0, 0.0, 0.0); + const interpolatecpp::path::CircularPath circular(axis, center, circle_pt); + const double half_arc = M_PI * circular.radius(); + sample_path(circular, 0.0, half_arc, 8, "CircularPath (half circle, r=3)"); +} + +// --------------------------------------------------------------------------- +// Example: QuaternionTrajectory with SquadC2, QuaternionSpline, and LogQuat +// --------------------------------------------------------------------------- + +static void example_quaternion_trajectory() { + ex::print_header("4. QuaternionTrajectory Concept"); + + std::cout << "The same template function 'sample_quaternion' works with any type\n" + << "that satisfies the QuaternionTrajectory concept.\n\n"; + + using interpolatecpp::quat::Quaternion; + + // Shared waypoints: identity -> 90deg Z -> 90deg X+Z -> identity + const Quaternion q1 = Quaternion::identity(); + const Quaternion q2 = Quaternion::from_euler_angles(0.0, 0.0, M_PI / 2.0); + const Quaternion q3 = Quaternion::from_euler_angles(M_PI / 2.0, 0.0, M_PI / 2.0); + const Quaternion q4 = Quaternion::identity(); + const std::vector times = {0.0, 1.0, 2.0, 3.0}; + const std::vector quats = {q1, q2, q3, q4}; + + // SquadC2 + const interpolatecpp::quat::SquadC2 squad(times, quats); + sample_quaternion(squad, 0.0, 3.0, 10, "SquadC2"); + + // QuaternionSpline (Squad method) + const interpolatecpp::quat::QuaternionSpline qspline( + times, quats, interpolatecpp::quat::QuaternionSpline::Method::Squad); + sample_quaternion(qspline, 0.0, 3.0, 10, "QuaternionSpline (SQUAD method)"); + + // LogQuaternionInterpolation + const interpolatecpp::quat::LogQuaternionInterpolation logquat(times, quats); + sample_quaternion(logquat, 0.0, 3.0, 10, "LogQuaternionInterpolation"); +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main() { + std::cout << "C++20 Concepts -- Compile-Time Protocol Equivalents\n"; + ex::print_separator('='); + + print_concept_conformance(); + example_scalar_trajectory(); + example_curve_evaluator(); + example_geometric_path(); + example_quaternion_trajectory(); + + std::cout << "All concept examples completed.\n"; + return 0; +} diff --git a/cpp/examples/cubic_smoothing_example.cpp b/cpp/examples/cubic_smoothing_example.cpp new file mode 100644 index 0000000..b0053e8 --- /dev/null +++ b/cpp/examples/cubic_smoothing_example.cpp @@ -0,0 +1,178 @@ +/// Cubic smoothing spline example โ€” C++ port of examples/c_s_smoothing_ex.py +/// AND examples/c_s_smoot_search_ex.py +/// +/// Part 1: Smoothing spline with varying mu values and weighted endpoints. +/// Part 2: Tolerance-based search for optimal smoothing parameter. + +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::spline; + +// --------------------------------------------------------------------------- +// Part 1: Smoothing splines with different mu values +// --------------------------------------------------------------------------- +void smoothing_mu_example() { + ex::print_header("Part 1: Smoothing Splines with Different mu Values"); + + // Define points from the textbook + std::vector t = {0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0}; + std::vector q = {3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0}; + + // Weights: infinite at endpoints (fixed), 1.0 elsewhere + const double inf = std::numeric_limits::infinity(); + std::vector weights = {inf, 1.0, 1.0, 1.0, 1.0, 1.0, inf}; + + // Create splines with different mu values + std::vector mu_values = {0.3, 0.6, 1.0}; + + for (double mu : mu_values) { + std::cout << "--- mu = " << mu << " ---\n\n"; + + CubicSmoothingSpline spline( + t, q, mu, + std::span(weights), + 0.0, 0.0); + + // Print smoothed vs original positions + std::cout << " Smoothed positions (s): "; + const auto& s = spline.s_points(); + std::cout << "["; + for (Eigen::Index i = 0; i < s.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << s(i); + } + std::cout << "]\n"; + + // Print deviation from original + std::cout << " Deviation (q - s): ["; + for (Eigen::Index i = 0; i < s.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << (q[static_cast(i)] - s(i)); + } + std::cout << "]\n"; + + ex::print_value("Lambda", spline.lambda()); + ex::print_value("Mu", spline.mu()); + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double tv) { return spline.evaluate(tv); }, + [&](double tv) { return spline.evaluate_velocity(tv); }, + [&](double tv) { return spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + } + + // Summary comparison: max deviation for each mu + ex::print_separator('='); + std::cout << "Summary: max deviation from waypoints\n\n"; + for (double mu : mu_values) { + CubicSmoothingSpline spline( + t, q, mu, + std::span(weights), + 0.0, 0.0); + + const auto& s = spline.s_points(); + double max_dev = 0.0; + for (Eigen::Index i = 0; i < s.size(); ++i) { + max_dev = std::max(max_dev, std::abs(q[static_cast(i)] - s(i))); + } + std::cout << " mu=" << std::fixed << std::setprecision(1) << mu + << " max deviation=" << std::setprecision(6) << max_dev << "\n"; + } +} + +// --------------------------------------------------------------------------- +// Part 2: Smoothing spline with prescribed tolerance (binary search) +// --------------------------------------------------------------------------- +void smoothing_tolerance_example() { + ex::print_header("Part 2: Smoothing Spline with Prescribed Tolerance"); + + // Same data as Part 1 + std::vector t = {0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0}; + std::vector q = {3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0}; + + // Weights with fixed endpoints + const double inf = std::numeric_limits::infinity(); + Eigen::VectorXd weights(7); + weights << inf, 1.0, 1.0, 1.0, 1.0, 1.0, inf; + + // Test different tolerance values + std::vector tolerances = {0.5, 1.0, 2.0}; + + SplineConfig config; + config.weights = weights; + config.v0 = 0.0; + config.vn = 0.0; + config.debug = false; + + for (double tol : tolerances) { + std::cout << "--- Tolerance = " << std::fixed << std::setprecision(1) + << tol << " ---\n\n"; + + SmoothingSearchResult result = + smoothing_spline_with_tolerance(t, q, tol, config); + + ex::print_value("Found mu", result.mu); + ex::print_value("Max error", result.max_error); + ex::print_value("Iterations", static_cast(result.iterations), 0); + + // Print smoothed positions + const auto& s = result.spline.s_points(); + std::cout << " Smoothed positions: ["; + for (Eigen::Index i = 0; i < s.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << s(i); + } + std::cout << "]\n"; + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double tv) { return result.spline.evaluate(tv); }, + [&](double tv) { return result.spline.evaluate_velocity(tv); }, + [&](double tv) { return result.spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + } + + // Summary + ex::print_separator('='); + std::cout << "Tolerance search summary:\n\n"; + const int w = 14; + std::cout << std::right + << std::setw(w) << "Tolerance" + << std::setw(w) << "Mu" + << std::setw(w) << "Max Error" + << std::setw(w) << "Iterations" + << "\n"; + ex::print_separator('-', 4 * w); + + for (double tol : tolerances) { + SmoothingSearchResult result = + smoothing_spline_with_tolerance(t, q, tol, config); + + std::cout << std::fixed + << std::setprecision(1) << std::setw(w) << tol + << std::setprecision(6) << std::setw(w) << result.mu + << std::setprecision(6) << std::setw(w) << result.max_error + << std::setprecision(0) << std::setw(w) << static_cast(result.iterations) + << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +int main() { + smoothing_mu_example(); + smoothing_tolerance_example(); + + return 0; +} diff --git a/cpp/examples/cubic_spline_acc1_example.cpp b/cpp/examples/cubic_spline_acc1_example.cpp new file mode 100644 index 0000000..8a54601 --- /dev/null +++ b/cpp/examples/cubic_spline_acc1_example.cpp @@ -0,0 +1,316 @@ +/// Cubic spline with acceleration constraints (method 1) โ€” C++ port of +/// examples/c_s_with_acc1_ex.py +/// +/// Demonstrates all 5 sub-examples: simple/textbook, robot joint, camera pan, +/// drone height with time scaling, and boundary condition comparison. + +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::spline; + +// --------------------------------------------------------------------------- +// Example 0: Simple textbook example +// --------------------------------------------------------------------------- +void simple_example() { + ex::print_header("Example 0: Textbook Example"); + + std::vector t = {0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0}; + std::vector q = {3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0}; + + CubicSplineWithAcceleration1 spline(t, q, 2.0, -3.0, 0.0, 0.0); + + ex::print_trajectory_table( + [&](double tv) { return spline.evaluate(tv); }, + [&](double tv) { return spline.evaluate_velocity(tv); }, + [&](double tv) { return spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); +} + +// --------------------------------------------------------------------------- +// Example 1: Robot joint motion planning +// --------------------------------------------------------------------------- +void robot_joint_example() { + ex::print_header("Example 1: Robot Joint Motion Planning"); + + // Joint angle waypoints (radians) and times + std::vector t = {0.0, 2.0, 4.0, 7.0, 10.0}; + std::vector q = {0.0, 1.57, 0.5, -0.5, 0.0}; + + // Zero initial/final velocity and acceleration for smooth start/stop + CubicSplineWithAcceleration1 spline(t, q, 0.0, 0.0, 0.0, 0.0); + + const double t_sample = 3.0; + ex::print_value("Position at t=3.0 (rad)", spline.evaluate(t_sample)); + ex::print_value("Velocity at t=3.0 (rad/s)", spline.evaluate_velocity(t_sample)); + ex::print_value("Acceleration at t=3.0 (rad/s^2)", spline.evaluate_acceleration(t_sample)); + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double tv) { return spline.evaluate(tv); }, + [&](double tv) { return spline.evaluate_velocity(tv); }, + [&](double tv) { return spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); +} + +// --------------------------------------------------------------------------- +// Example 2: Camera pan trajectory +// --------------------------------------------------------------------------- +void camera_pan_example() { + ex::print_header("Example 2: Camera Pan Trajectory"); + + // Camera angle waypoints (degrees) with times + std::vector t = {0.0, 3.0, 7.0, 12.0, 15.0}; + std::vector q = {0.0, 45.0, 90.0, 120.0, 180.0}; + + // Non-zero initial/final velocities for natural camera motion + CubicSplineWithAcceleration1 spline(t, q, 10.0, 5.0, 0.0, 0.0); + + // Print sampled trajectory table + const int n_samples = 15; + const int w = 16; + const int p = 2; + + std::cout << std::right + << std::setw(w) << "Time (s)" + << std::setw(w) << "Position (deg)" + << std::setw(w) << "Velocity (deg/s)" + << "\n"; + ex::print_separator('-', 3 * w); + + for (int i = 0; i <= n_samples; ++i) { + double tv = t.front() + (t.back() - t.front()) * static_cast(i) / n_samples; + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << tv + << std::setw(w) << spline.evaluate(tv) + << std::setw(w) << spline.evaluate_velocity(tv) + << "\n"; + } + std::cout << "\n"; +} + +// --------------------------------------------------------------------------- +// Example 3: Drone height trajectory with time scaling +// --------------------------------------------------------------------------- +void drone_height_example() { + ex::print_header("Example 3: Drone Height Trajectory with Time Scaling"); + + std::vector t = {0.0, 5.0, 10.0, 20.0, 25.0}; + std::vector q = {0.0, 10.0, 15.0, 5.0, 2.0}; + + // Limited initial/final accelerations + CubicSplineWithAcceleration1 spline(t, q, 0.0, 0.0, 0.5, -0.5); + + // Time-scaled version (1.5x slower) + const double time_scale = 1.5; + std::vector t_scaled(t.size()); + for (size_t i = 0; i < t.size(); ++i) { + t_scaled[i] = t[i] * time_scale; + } + CubicSplineWithAcceleration1 spline_scaled(t_scaled, q, 0.0, 0.0, 0.5, -0.5); + + // Print original trajectory + std::cout << "--- Original trajectory ---\n"; + ex::print_trajectory_table( + [&](double tv) { return spline.evaluate(tv); }, + [&](double tv) { return spline.evaluate_velocity(tv); }, + [&](double tv) { return spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Print time-scaled trajectory + std::cout << "--- Time-scaled trajectory (1.5x) ---\n"; + ex::print_trajectory_table( + [&](double tv) { return spline_scaled.evaluate(tv); }, + [&](double tv) { return spline_scaled.evaluate_velocity(tv); }, + [&](double tv) { return spline_scaled.evaluate_acceleration(tv); }, + t_scaled.front(), t_scaled.back(), 15); + + // Summary + ex::print_value("Original duration (s)", t.back(), 1); + ex::print_value("Scaled duration (s)", t_scaled.back(), 1); + + // Compute max absolute acceleration for both + const int n_eval = 100; + double max_acc_orig = 0.0; + double max_acc_scaled = 0.0; + for (int i = 0; i <= n_eval; ++i) { + double frac = static_cast(i) / n_eval; + double tv_orig = t.front() + (t.back() - t.front()) * frac; + double tv_sc = t_scaled.front() + (t_scaled.back() - t_scaled.front()) * frac; + max_acc_orig = std::max(max_acc_orig, std::abs(spline.evaluate_acceleration(tv_orig))); + max_acc_scaled = std::max(max_acc_scaled, std::abs(spline_scaled.evaluate_acceleration(tv_sc))); + } + ex::print_value("Max acceleration - original (m/s^2)", max_acc_orig, 2); + ex::print_value("Max acceleration - scaled (m/s^2)", max_acc_scaled, 2); +} + +// --------------------------------------------------------------------------- +// Example 4: Multi-dimensional trajectory (3D path) +// --------------------------------------------------------------------------- +void multi_dimensional_example() { + ex::print_header("Example 4: Multi-dimensional Trajectory (3D Path)"); + + std::vector t = {0.0, 2.0, 5.0, 8.0, 10.0, 15.0}; + std::vector x_pts = {0.0, 2.0, 5.0, 8.0, 6.0, 10.0}; + std::vector y_pts = {0.0, 3.0, 8.0, 4.0, 2.0, 5.0}; + std::vector z_pts = {0.0, 1.0, 2.0, 4.0, 3.0, 1.0}; + + CubicSplineWithAcceleration1 sx(t, x_pts, 0.0, 0.0, 0.0, 0.0); + CubicSplineWithAcceleration1 sy(t, y_pts, 0.0, 0.0, 0.0, 0.0); + CubicSplineWithAcceleration1 sz(t, z_pts, 0.0, 0.0, 0.0, 0.0); + + // Print 3D trajectory table + const int n_samples = 15; + const int w = 12; + const int p = 4; + + std::cout << std::right + << std::setw(w) << "Time" + << std::setw(w) << "X" + << std::setw(w) << "Y" + << std::setw(w) << "Z" + << std::setw(w) << "Speed" + << "\n"; + ex::print_separator('-', 5 * w); + + double total_path = 0.0; + double max_speed = 0.0; + double sum_speed = 0.0; + double prev_x = sx.evaluate(t.front()); + double prev_y = sy.evaluate(t.front()); + double prev_z = sz.evaluate(t.front()); + + for (int i = 0; i <= n_samples; ++i) { + double tv = t.front() + (t.back() - t.front()) * static_cast(i) / n_samples; + double xv = sx.evaluate(tv); + double yv = sy.evaluate(tv); + double zv = sz.evaluate(tv); + double vx = sx.evaluate_velocity(tv); + double vy = sy.evaluate_velocity(tv); + double vz = sz.evaluate_velocity(tv); + double speed = std::sqrt(vx * vx + vy * vy + vz * vz); + + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << tv + << std::setw(w) << xv + << std::setw(w) << yv + << std::setw(w) << zv + << std::setw(w) << speed + << "\n"; + + if (i > 0) { + double dx = xv - prev_x; + double dy = yv - prev_y; + double dz = zv - prev_z; + total_path += std::sqrt(dx * dx + dy * dy + dz * dz); + } + max_speed = std::max(max_speed, speed); + sum_speed += speed; + prev_x = xv; + prev_y = yv; + prev_z = zv; + } + + std::cout << "\n"; + ex::print_value("Total path length (approx)", total_path, 2); + ex::print_value("Maximum speed", max_speed, 2); + ex::print_value("Average speed", sum_speed / (n_samples + 1), 2); +} + +// --------------------------------------------------------------------------- +// Example 5: Comparing different boundary conditions +// --------------------------------------------------------------------------- +void compare_boundary_conditions() { + ex::print_header("Example 5: Comparing Different Boundary Conditions"); + + std::vector t = {0.0, 4.0, 8.0, 12.0, 16.0}; + std::vector q = {0.0, 10.0, 5.0, 15.0, 10.0}; + + // Also create a basic CubicSpline for reference + CubicSpline basic_spline(t, q, 0.0, 0.0); + std::cout << "Reference: CubicSpline (no accel constraints)\n"; + ex::print_trajectory_table( + [&](double tv) { return basic_spline.evaluate(tv); }, + [&](double tv) { return basic_spline.evaluate_velocity(tv); }, + [&](double tv) { return basic_spline.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Spline 1: v0=vn=0, a0=an=0 + CubicSplineWithAcceleration1 s1(t, q, 0.0, 0.0, 0.0, 0.0); + std::cout << "Spline 1: v0=vn=0, a0=an=0\n"; + ex::print_trajectory_table( + [&](double tv) { return s1.evaluate(tv); }, + [&](double tv) { return s1.evaluate_velocity(tv); }, + [&](double tv) { return s1.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Spline 2: v0=2, vn=-2, a0=an=0 + CubicSplineWithAcceleration1 s2(t, q, 2.0, -2.0, 0.0, 0.0); + std::cout << "Spline 2: v0=2, vn=-2, a0=an=0\n"; + ex::print_trajectory_table( + [&](double tv) { return s2.evaluate(tv); }, + [&](double tv) { return s2.evaluate_velocity(tv); }, + [&](double tv) { return s2.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Spline 3: v0=vn=0, a0=1, an=-1 + CubicSplineWithAcceleration1 s3(t, q, 0.0, 0.0, 1.0, -1.0); + std::cout << "Spline 3: v0=vn=0, a0=1, an=-1\n"; + ex::print_trajectory_table( + [&](double tv) { return s3.evaluate(tv); }, + [&](double tv) { return s3.evaluate_velocity(tv); }, + [&](double tv) { return s3.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Spline 4: v0=2, vn=-2, a0=1, an=-1 + CubicSplineWithAcceleration1 s4(t, q, 2.0, -2.0, 1.0, -1.0); + std::cout << "Spline 4: v0=2, vn=-2, a0=1, an=-1\n"; + ex::print_trajectory_table( + [&](double tv) { return s4.evaluate(tv); }, + [&](double tv) { return s4.evaluate_velocity(tv); }, + [&](double tv) { return s4.evaluate_acceleration(tv); }, + t.front(), t.back(), 15); + + // Smoothness comparison via approximate jerk integral + ex::print_separator('='); + std::cout << "Smoothness comparison (sum of squared jerk, smaller is better):\n\n"; + + auto compute_jerk_metric = [&](auto& spline) { + const int n = 1000; + const double dt = (t.back() - t.front()) / n; + double metric = 0.0; + double prev_acc = spline.evaluate_acceleration(t.front()); + for (int i = 1; i <= n; ++i) { + double tv = t.front() + dt * i; + double acc = spline.evaluate_acceleration(tv); + double jerk = (acc - prev_acc) / dt; + metric += jerk * jerk; + prev_acc = acc; + } + return metric; + }; + + ex::print_value("Spline 1 (v0=vn=0, a0=an=0)", compute_jerk_metric(s1), 2); + ex::print_value("Spline 2 (v0=2, vn=-2, a0=an=0)", compute_jerk_metric(s2), 2); + ex::print_value("Spline 3 (v0=vn=0, a0=1, an=-1)", compute_jerk_metric(s3), 2); + ex::print_value("Spline 4 (v0=2, vn=-2, a0=1, an=-1)", compute_jerk_metric(s4), 2); +} + +// --------------------------------------------------------------------------- +int main() { + simple_example(); + robot_joint_example(); + camera_pan_example(); + drone_height_example(); + multi_dimensional_example(); + compare_boundary_conditions(); + + return 0; +} diff --git a/cpp/examples/cubic_spline_acc2_example.cpp b/cpp/examples/cubic_spline_acc2_example.cpp new file mode 100644 index 0000000..397043d --- /dev/null +++ b/cpp/examples/cubic_spline_acc2_example.cpp @@ -0,0 +1,113 @@ +/// Cubic spline with acceleration constraints (method 2, quintic segments) โ€” +/// C++ port of examples/c_s_with_acc2_ex.py +/// +/// Demonstrates CubicSplineWithAcceleration2 with SplineParameters, +/// trajectory evaluation, and boundary condition verification. + +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::spline; + +int main() { + ex::print_header("Cubic Spline with Acceleration (Method 2) Example"); + + // Define waypoints + std::vector t_points = {0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0}; + std::vector q_points = {3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0}; + + // Create parameters object + SplineParameters params; + params.v0 = 2.0; // Initial velocity + params.vn = -3.0; // Final velocity + params.a0 = 0.0; // Initial acceleration + params.an = 0.0; // Final acceleration + params.debug = false; + + // Create cubic spline with quintic boundary segments + CubicSplineWithAcceleration2 spline(t_points, q_points, params); + + // Print trajectory table + std::cout << "Trajectory evaluation (15 samples):\n\n"; + ex::print_trajectory_table( + [&](double t) { return spline.evaluate(t); }, + [&](double t) { return spline.evaluate_velocity(t); }, + [&](double t) { return spline.evaluate_acceleration(t); }, + t_points.front(), t_points.back(), 15); + + // Verify boundary conditions + ex::print_separator('='); + std::cout << "Verifying boundary conditions:\n\n"; + + const double initial_vel = spline.evaluate_velocity(t_points.front()); + ex::print_value("Initial velocity", initial_vel); + ex::print_value("Expected v0", params.v0); + + const double final_vel = spline.evaluate_velocity(t_points.back()); + ex::print_value("Final velocity", final_vel); + ex::print_value("Expected vn", params.vn); + + const double initial_acc = spline.evaluate_acceleration(t_points.front()); + const double expected_a0 = params.a0.value_or(0.0); + ex::print_value("Initial acceleration", initial_acc); + ex::print_value("Expected a0", expected_a0); + + const double final_acc = spline.evaluate_acceleration(t_points.back()); + const double expected_an = params.an.value_or(0.0); + ex::print_value("Final acceleration", final_acc); + ex::print_value("Expected an", expected_an); + + // Report quintic segment presence + ex::print_separator('='); + std::cout << "Quintic segment info:\n\n"; + std::cout << " Has quintic first segment: " << (spline.has_quintic_first() ? "yes" : "no") << "\n"; + std::cout << " Has quintic last segment: " << (spline.has_quintic_last() ? "yes" : "no") << "\n"; + + // Verify errors are small + std::cout << "\n"; + const double vel_err_start = std::abs(initial_vel - params.v0); + const double vel_err_end = std::abs(final_vel - params.vn); + const double acc_err_start = std::abs(initial_acc - expected_a0); + const double acc_err_end = std::abs(final_acc - expected_an); + + ex::print_value("Velocity error at start", vel_err_start); + ex::print_value("Velocity error at end", vel_err_end); + ex::print_value("Acceleration error at start", acc_err_start); + ex::print_value("Acceleration error at end", acc_err_end); + + // Demonstrate with non-zero acceleration constraints + ex::print_header("Variant: Non-zero acceleration constraints"); + + SplineParameters params2; + params2.v0 = 1.0; + params2.vn = -1.0; + params2.a0 = 2.0; + params2.an = -2.0; + + CubicSplineWithAcceleration2 spline2(t_points, q_points, params2); + + ex::print_trajectory_table( + [&](double t) { return spline2.evaluate(t); }, + [&](double t) { return spline2.evaluate_velocity(t); }, + [&](double t) { return spline2.evaluate_acceleration(t); }, + t_points.front(), t_points.back(), 15); + + std::cout << "Boundary verification (non-zero accel):\n\n"; + ex::print_value("Initial velocity", spline2.evaluate_velocity(t_points.front())); + ex::print_value("Expected v0", params2.v0); + ex::print_value("Final velocity", spline2.evaluate_velocity(t_points.back())); + ex::print_value("Expected vn", params2.vn); + ex::print_value("Initial acceleration", spline2.evaluate_acceleration(t_points.front())); + ex::print_value("Expected a0", params2.a0.value_or(0.0)); + ex::print_value("Final acceleration", spline2.evaluate_acceleration(t_points.back())); + ex::print_value("Expected an", params2.an.value_or(0.0)); + + return 0; +} diff --git a/cpp/examples/cubic_spline_example.cpp b/cpp/examples/cubic_spline_example.cpp new file mode 100644 index 0000000..1a60e94 --- /dev/null +++ b/cpp/examples/cubic_spline_example.cpp @@ -0,0 +1,67 @@ +/// Cubic spline example โ€” C++ port of examples/cubic_spline_ex.py +/// +/// Demonstrates basic CubicSpline construction with boundary velocities, +/// trajectory evaluation, and accessor inspection. + +#include + +#include "example_utils.hpp" + +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::spline; + +int main() { + ex::print_header("Cubic Spline Example"); + + // Define waypoints (same as the Python example) + std::vector t_points = {0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0}; + std::vector q_points = {3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0}; + + // Create cubic spline with initial and final velocities + const double v0 = 2.0; + const double vn = -3.0; + CubicSpline spline(t_points, q_points, v0, vn); + + // Print trajectory table at 15 samples + std::cout << "Trajectory evaluation (15 samples):\n\n"; + ex::print_trajectory_table( + [&](double t) { return spline.evaluate(t); }, + [&](double t) { return spline.evaluate_velocity(t); }, + [&](double t) { return spline.evaluate_acceleration(t); }, + t_points.front(), t_points.back(), 15); + + // Print accessors + ex::print_separator('='); + std::cout << "Spline properties:\n\n"; + ex::print_value("Number of segments", static_cast(spline.n_segments()), 0); + ex::print_value("Initial velocity (v0)", v0); + ex::print_value("Final velocity (vn)", vn); + + std::cout << "\n"; + ex::print_vector("Time points", spline.t_points()); + ex::print_vector("Position points", spline.q_points()); + ex::print_vector("Computed velocities", spline.velocities()); + + std::cout << "\n"; + ex::print_matrix("Coefficients (a0, a1, a2, a3 per segment)", spline.coefficients()); + + // Verify boundary velocities + ex::print_separator('='); + std::cout << "Boundary verification:\n\n"; + ex::print_value("Velocity at t_start", spline.evaluate_velocity(t_points.front())); + ex::print_value("Expected v0", v0); + ex::print_value("Velocity at t_end", spline.evaluate_velocity(t_points.back())); + ex::print_value("Expected vn", vn); + + // Evaluate at a specific point + std::cout << "\n"; + const double t_sample = 6.0; + ex::print_value("Position at t=6.0", spline.evaluate(t_sample)); + ex::print_value("Velocity at t=6.0", spline.evaluate_velocity(t_sample)); + ex::print_value("Acceleration at t=6.0", spline.evaluate_acceleration(t_sample)); + + return 0; +} diff --git a/cpp/examples/double_s_example.cpp b/cpp/examples/double_s_example.cpp new file mode 100644 index 0000000..28d06ec --- /dev/null +++ b/cpp/examples/double_s_example.cpp @@ -0,0 +1,222 @@ +/// Double-S trajectory example -- C++ port of examples/double_s_ex.py +/// +/// Demonstrates double-S (7-phase) trajectory with bounded jerk: +/// 1. Standard trajectory (q0=0, q1=30, v0=0, v1=0) +/// 2. Velocity matching (same position, different velocity) +/// 3. Negative displacement +/// 4. Asymmetric velocities +/// 5. Phase duration inspection + +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::motion; + +static void print_phase_durations(const std::map& phases) { + std::cout << " Phase durations:\n"; + for (const auto& [name, dur] : phases) { + std::cout << " " << std::left << std::setw(24) << name + << ": " << std::fixed << std::setprecision(6) << dur << " s\n"; + } + std::cout << std::right; +} + +static void example_standard() { + ex::print_header("Example 1: Standard Double-S Trajectory"); + + std::cout << " q0=0, q1=30, v0=0, v1=0\n"; + std::cout << " Bounds: v=20, a=60, j=120\n\n"; + + StateParams state{0.0, 30.0, 0.0, 0.0}; + TrajectoryBounds bounds(20.0, 60.0, 120.0); + + DoubleSTrajectory traj(state, bounds); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 3); + + // Evaluate at start and end + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Start position", r_start.position, 3); + ex::print_value("Start velocity", r_start.velocity, 3); + ex::print_value("End position", r_end.position, 3); + ex::print_value("End velocity", r_end.velocity, 3); + + // Phase durations + std::cout << "\n"; + auto phases = traj.phase_durations(); + print_phase_durations(phases); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + 0.0, dur, 20); +} + +static void example_velocity_matching() { + ex::print_header("Example 2: Velocity Matching (Same Position)"); + + std::cout << " q0=10, q1=10, v0=0, v1=5\n"; + std::cout << " Bounds: v=20, a=60, j=120\n\n"; + + StateParams state{10.0, 10.0, 0.0, 5.0}; + TrajectoryBounds bounds(20.0, 60.0, 120.0); + + DoubleSTrajectory traj(state, bounds); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 3); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Start position", r_start.position, 3); + ex::print_value("Start velocity", r_start.velocity, 3); + ex::print_value("End position", r_end.position, 3); + ex::print_value("End velocity", r_end.velocity, 3); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + 0.0, dur, 15); +} + +static void example_negative_displacement() { + ex::print_header("Example 3: Negative Displacement"); + + std::cout << " q0=0, q1=-30, v0=0, v1=0\n"; + std::cout << " Bounds: v=20, a=60, j=120\n\n"; + + StateParams state{0.0, -30.0, 0.0, 0.0}; + TrajectoryBounds bounds(20.0, 60.0, 120.0); + + DoubleSTrajectory traj(state, bounds); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 3); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Start position", r_start.position, 3); + ex::print_value("End position", r_end.position, 3); + + // Phase durations + std::cout << "\n"; + auto phases = traj.phase_durations(); + print_phase_durations(phases); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + 0.0, dur, 15); +} + +static void example_asymmetric_velocities() { + ex::print_header("Example 4: Asymmetric Velocities"); + + std::cout << " q0=0, q1=50, v0=10, v1=-5\n"; + std::cout << " Bounds: v=20, a=60, j=120\n\n"; + + StateParams state{0.0, 50.0, 10.0, -5.0}; + TrajectoryBounds bounds(20.0, 60.0, 120.0); + + DoubleSTrajectory traj(state, bounds); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 3); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Start position", r_start.position, 3); + ex::print_value("Start velocity", r_start.velocity, 3); + ex::print_value("End position", r_end.position, 3); + ex::print_value("End velocity", r_end.velocity, 3); + + // Phase durations + std::cout << "\n"; + auto phases = traj.phase_durations(); + print_phase_durations(phases); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + 0.0, dur, 20); +} + +static void example_phase_durations() { + ex::print_header("Example 5: Phase Duration Comparison"); + + std::cout << " Comparing phase structure across different scenarios.\n\n"; + + struct Scenario { + const char* name; + StateParams state; + TrajectoryBounds bounds; + }; + + const Scenario scenarios[] = { + {"Short displacement", + StateParams{0.0, 5.0, 0.0, 0.0}, + TrajectoryBounds(20.0, 60.0, 120.0)}, + {"Long displacement", + StateParams{0.0, 100.0, 0.0, 0.0}, + TrajectoryBounds(20.0, 60.0, 120.0)}, + {"High jerk limit", + StateParams{0.0, 30.0, 0.0, 0.0}, + TrajectoryBounds(20.0, 60.0, 500.0)}, + {"Low velocity bound", + StateParams{0.0, 30.0, 0.0, 0.0}, + TrajectoryBounds(5.0, 60.0, 120.0)}, + }; + + for (const auto& [name, state, bounds] : scenarios) { + DoubleSTrajectory traj(state, bounds); + std::cout << " " << name << ":\n"; + ex::print_value(" Total duration", traj.duration(), 4); + + auto phases = traj.phase_durations(); + for (const auto& [phase_name, phase_dur] : phases) { + std::cout << " " << std::left << std::setw(24) << phase_name + << ": " << std::fixed << std::setprecision(4) << phase_dur << " s\n"; + } + std::cout << std::right << "\n"; + } +} + +int main() { + std::cout << "Double-S Trajectory -- C++ Usage Examples\n"; + ex::print_separator('='); + + example_standard(); + example_velocity_matching(); + example_negative_displacement(); + example_asymmetric_velocities(); + example_phase_durations(); + + std::cout << "\nAll double-S trajectory examples completed.\n"; + return 0; +} diff --git a/cpp/examples/parabolic_linear_example.cpp b/cpp/examples/parabolic_linear_example.cpp new file mode 100644 index 0000000..85e94e8 --- /dev/null +++ b/cpp/examples/parabolic_linear_example.cpp @@ -0,0 +1,206 @@ +/// Parabolic blend and linear trajectory example -- C++ port of +/// examples/lin_poly_parabolic_ex.py and examples/linear_ex.py +/// +/// Part 1: Linear trajectory interpolation (scalar and 2D) +/// Part 2: Parabolic blend trajectory with waypoints + +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::motion; +using namespace interpolatecpp::path; + +// ===================================================================== +// Part 1: Linear Trajectory +// ===================================================================== + +static void example_linear_scalar() { + ex::print_header("Part 1a: Linear Trajectory -- Scalar Case"); + + std::cout << " p0=0, p1=10, t=[0, 2], 100 points\n\n"; + + Eigen::VectorXd p0(1); + p0 << 0.0; + Eigen::VectorXd p1(1); + p1 << 10.0; + + auto result = linear_traj(p0, p1, 0.0, 2.0, 100); + + // result.positions is (100 x 1) + ex::print_value("Start position", result.positions(0, 0), 4); + ex::print_value("End position", result.positions(result.positions.rows() - 1, 0), 4); + ex::print_value("Constant velocity", result.velocities(0, 0), 4); + ex::print_value("Acceleration", result.accelerations(0, 0), 4); + + // Print a few sample rows + std::cout << "\n Sample points (every 20th):\n"; + std::cout << std::right << std::setw(10) << "Index" + << std::setw(14) << "Position" + << std::setw(14) << "Velocity" + << std::setw(14) << "Acceleration" << "\n"; + ex::print_separator('-', 52); + + for (int i = 0; i < result.positions.rows(); i += 20) { + std::cout << std::setw(10) << i + << std::fixed << std::setprecision(4) + << std::setw(14) << result.positions(i, 0) + << std::setw(14) << result.velocities(i, 0) + << std::setw(14) << result.accelerations(i, 0) << "\n"; + } + // Always print last row + { + int last = static_cast(result.positions.rows()) - 1; + std::cout << std::setw(10) << last + << std::fixed << std::setprecision(4) + << std::setw(14) << result.positions(last, 0) + << std::setw(14) << result.velocities(last, 0) + << std::setw(14) << result.accelerations(last, 0) << "\n"; + } + std::cout << "\n"; +} + +static void example_linear_vector() { + ex::print_header("Part 1b: Linear Trajectory -- 2D Vector Case"); + + std::cout << " p0=(0,0), p1=(10,5), t=[0, 3], 100 points\n\n"; + + Eigen::VectorXd p0(2); + p0 << 0.0, 0.0; + Eigen::VectorXd p1(2); + p1 << 10.0, 5.0; + + auto result = linear_traj(p0, p1, 0.0, 3.0, 100); + + // result.positions is (100 x 2) + std::cout << " Start position: (" << result.positions(0, 0) << ", " + << result.positions(0, 1) << ")\n"; + int last = static_cast(result.positions.rows()) - 1; + std::cout << " End position: (" << result.positions(last, 0) << ", " + << result.positions(last, 1) << ")\n"; + std::cout << " Velocity: (" << result.velocities(0, 0) << ", " + << result.velocities(0, 1) << ")\n"; + std::cout << " Acceleration: (" << result.accelerations(0, 0) << ", " + << result.accelerations(0, 1) << ")\n"; + + // Print sample points + std::cout << "\n Sample points (every 20th):\n"; + std::cout << std::right << std::setw(10) << "Index" + << std::setw(12) << "X pos" + << std::setw(12) << "Y pos" + << std::setw(12) << "X vel" + << std::setw(12) << "Y vel" << "\n"; + ex::print_separator('-', 58); + + for (int i = 0; i < result.positions.rows(); i += 20) { + std::cout << std::setw(10) << i + << std::fixed << std::setprecision(4) + << std::setw(12) << result.positions(i, 0) + << std::setw(12) << result.positions(i, 1) + << std::setw(12) << result.velocities(i, 0) + << std::setw(12) << result.velocities(i, 1) << "\n"; + } + std::cout << std::setw(10) << last + << std::fixed << std::setprecision(4) + << std::setw(12) << result.positions(last, 0) + << std::setw(12) << result.positions(last, 1) + << std::setw(12) << result.velocities(last, 0) + << std::setw(12) << result.velocities(last, 1) << "\n"; + std::cout << "\n"; +} + +// ===================================================================== +// Part 2: Parabolic Blend Trajectory +// ===================================================================== + +static void example_parabolic_blend() { + ex::print_header("Part 2: Parabolic Blend Trajectory"); + + const double pi = std::acos(-1.0); + + const std::vector q = {0.0, 2.0 * pi, pi / 2.0, pi}; + const std::vector t = {0.0, 2.0, 3.0, 5.0}; + const std::vector dt_blend = {0.6, 0.6, 0.6, 0.6}; + + std::cout << " Waypoints: ["; + for (size_t i = 0; i < q.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << q[i]; + } + std::cout << "]\n"; + std::cout << " Times: ["; + for (size_t i = 0; i < t.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << t[i]; + } + std::cout << "]\n"; + std::cout << " Blend durations: ["; + for (size_t i = 0; i < dt_blend.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << dt_blend[i]; + } + std::cout << "]\n\n"; + + ParabolicBlendTrajectory traj(q, t, dt_blend); + + const double dur = traj.duration(); + ex::print_value("Total duration", dur, 2); + ex::print_value("Number of waypoints", static_cast(traj.n_waypoints()), 0); + + // Evaluate at specific times (matching the Python example) + const std::vector eval_times = {0.5, 2.1, 3.5, 4.8}; + + std::cout << "\n Evaluating at specific times:\n"; + std::cout << std::right + << std::setw(12) << "Time" + << std::setw(14) << "Position" + << std::setw(14) << "Velocity" + << std::setw(14) << "Acceleration" << "\n"; + ex::print_separator('-', 54); + + for (double time_pt : eval_times) { + auto r = traj.evaluate(time_pt); + std::cout << std::fixed << std::setprecision(2) << std::setw(12) << time_pt + << std::setprecision(4) + << std::setw(14) << r.position + << std::setw(14) << r.velocity + << std::setw(14) << r.acceleration << "\n"; + } + + // Full trajectory table + std::cout << "\n Full trajectory profile:\n\n"; + ex::print_trajectory_table( + [&](double tv) { return traj.evaluate(tv).position; }, + [&](double tv) { return traj.evaluate(tv).velocity; }, + [&](double tv) { return traj.evaluate(tv).acceleration; }, + 0.0, dur, 20); + + // Out-of-bounds test + std::cout << " Out-of-bounds evaluation (t = duration + 1.0):\n"; + auto r_oob = traj.evaluate(dur + 1.0); + ex::print_value("Position", r_oob.position, 4); + ex::print_value("Velocity", r_oob.velocity, 4); + ex::print_value("Acceleration", r_oob.acceleration, 4); +} + +int main() { + std::cout << "Parabolic Blend & Linear Trajectory -- C++ Usage Examples\n"; + ex::print_separator('='); + + // Part 1: Linear trajectory + example_linear_scalar(); + example_linear_vector(); + + // Part 2: Parabolic blend + example_parabolic_blend(); + + std::cout << "\nAll parabolic blend and linear trajectory examples completed.\n"; + return 0; +} diff --git a/cpp/examples/paths_example.cpp b/cpp/examples/paths_example.cpp new file mode 100644 index 0000000..168bd55 --- /dev/null +++ b/cpp/examples/paths_example.cpp @@ -0,0 +1,326 @@ +/// Paths and Frenet frames example -- C++ port of examples/simple_paths_ex.py +/// and examples/frenet_frame_ex.py +/// +/// Demonstrates LinearPath, CircularPath, and Frenet-Serret frame computation +/// on helicoidal and circular trajectories. + +#include +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::path; + +// --------------------------------------------------------------------------- +// 1. LinearPath -- evaluate position, velocity, acceleration at arc-length samples +// --------------------------------------------------------------------------- + +static void linear_path_example() { + ex::print_header("Linear Path Example"); + + const Eigen::Vector3d pi(0.0, 0.0, 0.0); + const Eigen::Vector3d pf(10.0, 5.0, 3.0); + const LinearPath path(pi, pf); + + const double path_length = path.length(); + ex::print_value("Path length", path_length, 4); + ex::print_vector3("Start point", pi); + ex::print_vector3("End point", pf); + + // Evaluate at 10 arc-length samples + const int n_samples = 10; + const int w = 14; + const int p = 6; + + std::cout << "\nTrajectory along linear path (" << n_samples << " samples):\n\n"; + + std::cout << std::right + << std::setw(w) << "s" + << std::setw(w) << "Pos X" + << std::setw(w) << "Pos Y" + << std::setw(w) << "Pos Z" + << std::setw(w) << "Vel X" + << std::setw(w) << "Vel Y" + << std::setw(w) << "Vel Z" + << "\n"; + ex::print_separator('-', 7 * w); + + for (int i = 0; i <= n_samples; ++i) { + const double s = path_length * static_cast(i) / n_samples; + const Eigen::Vector3d pos = path.position(s); + const Eigen::Vector3d vel = path.velocity(s); + + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << s + << std::setw(w) << pos.x() + << std::setw(w) << pos.y() + << std::setw(w) << pos.z() + << std::setw(w) << vel.x() + << std::setw(w) << vel.y() + << std::setw(w) << vel.z() + << "\n"; + } + + // Acceleration is zero for a linear path + ex::print_separator('='); + std::cout << "Linear path properties:\n"; + ex::print_vector3("Acceleration (constant)", path.acceleration(0.0)); + std::cout << " Note: acceleration is zero for a straight-line path.\n"; + + // Vectorized evaluation + Eigen::VectorXd s_vec(5); + s_vec << 0.0, path_length * 0.25, path_length * 0.5, path_length * 0.75, path_length; + const Eigen::MatrixXd positions = path.position(s_vec); + + std::cout << "\nVectorized evaluation at 5 points:\n"; + ex::print_matrix("Positions", positions); +} + +// --------------------------------------------------------------------------- +// 2. CircularPath -- arc-length parameterized circle in 3D +// --------------------------------------------------------------------------- + +static void circular_path_example() { + ex::print_header("Circular Path Example"); + + const Eigen::Vector3d axis(0.0, 0.0, 1.0); + const Eigen::Vector3d axis_point(0.0, 0.0, 0.0); + const Eigen::Vector3d circle_point(2.0, 0.0, 0.0); + const CircularPath path(axis, axis_point, circle_point); + + const double radius = path.radius(); + const double half_arc = M_PI * radius; + + ex::print_value("Radius", radius, 4); + ex::print_vector3("Center", path.center()); + ex::print_value("Half-circle arc length", half_arc, 4); + + // Evaluate at angular samples over a full circle + const int n_samples = 12; + const double full_arc = 2.0 * M_PI * radius; + const int w = 14; + const int p = 6; + + std::cout << "\nTrajectory along circular path (" << n_samples << " angular samples):\n\n"; + + std::cout << std::right + << std::setw(w) << "s" + << std::setw(w) << "Pos X" + << std::setw(w) << "Pos Y" + << std::setw(w) << "Pos Z" + << std::setw(w) << "Vel X" + << std::setw(w) << "Vel Y" + << std::setw(w) << "Vel Z" + << "\n"; + ex::print_separator('-', 7 * w); + + for (int i = 0; i <= n_samples; ++i) { + const double s = full_arc * static_cast(i) / n_samples; + const Eigen::Vector3d pos = path.position(s); + const Eigen::Vector3d vel = path.velocity(s); + + std::cout << std::fixed << std::setprecision(p) + << std::setw(w) << s + << std::setw(w) << pos.x() + << std::setw(w) << pos.y() + << std::setw(w) << pos.z() + << std::setw(w) << vel.x() + << std::setw(w) << vel.y() + << std::setw(w) << vel.z() + << "\n"; + } + + // Print acceleration at a few points + ex::print_separator('='); + std::cout << "Circular path acceleration (centripetal, always points inward):\n\n"; + const std::vector check_points = {0.0, half_arc * 0.5, half_arc, half_arc * 1.5}; + for (const double s : check_points) { + const Eigen::Vector3d acc = path.acceleration(s); + std::cout << " s = " << std::fixed << std::setprecision(2) << s + << " -> acc = (" << std::setprecision(4) + << acc.x() << ", " << acc.y() << ", " << acc.z() << ")" + << " |acc| = " << acc.norm() << "\n"; + } +} + +// --------------------------------------------------------------------------- +// 3. Frenet frames on helicoidal trajectory +// --------------------------------------------------------------------------- + +static void frenet_frame_helicoidal() { + ex::print_header("Frenet Frames -- Helicoidal Trajectory"); + + const double r = 2.0; + const double d = 0.5; + const int n_samples = 8; + + std::cout << "Parameters: r = " << r << ", d = " << d << "\n"; + std::cout << "Helicoidal curve: p(u) = (r*cos(u), r*sin(u), d*u)\n\n"; + + // Build parameter values + Eigen::VectorXd u_values(n_samples); + for (int i = 0; i < n_samples; ++i) { + u_values(i) = 4.0 * M_PI * static_cast(i) / (n_samples - 1); + } + + // Curve function returning (position, velocity, acceleration) + auto helix_fn = [r, d](double u) + -> std::tuple { + return helicoidal_trajectory_with_derivatives(u, r, d); + }; + + const std::vector frames = compute_frenet_frames(helix_fn, u_values); + + // Print table of Frenet frame data + const int w = 12; + const int p = 4; + + std::cout << std::right + << std::setw(8) << "u" + << std::setw(w) << "T.x" + << std::setw(w) << "T.y" + << std::setw(w) << "T.z" + << std::setw(w) << "N.x" + << std::setw(w) << "N.y" + << std::setw(w) << "N.z" + << std::setw(w) << "B.x" + << std::setw(w) << "B.y" + << std::setw(w) << "B.z" + << std::setw(w) << "curv" + << std::setw(w) << "torsion" + << "\n"; + ex::print_separator('-', 8 + 11 * w); + + for (int i = 0; i < n_samples; ++i) { + const auto& f = frames[static_cast(i)]; + std::cout << std::fixed << std::setprecision(p) + << std::setw(8) << u_values(i) + << std::setw(w) << f.tangent.x() + << std::setw(w) << f.tangent.y() + << std::setw(w) << f.tangent.z() + << std::setw(w) << f.normal.x() + << std::setw(w) << f.normal.y() + << std::setw(w) << f.normal.z() + << std::setw(w) << f.binormal.x() + << std::setw(w) << f.binormal.y() + << std::setw(w) << f.binormal.z() + << std::setw(w) << f.curvature + << std::setw(w) << f.torsion + << "\n"; + } + + // Print summary for first and last frames + ex::print_separator('='); + std::cout << "Frame details:\n\n"; + + const auto& first = frames.front(); + std::cout << " First frame (u = 0):\n"; + ex::print_vector3(" Tangent", first.tangent); + ex::print_vector3(" Normal", first.normal); + ex::print_vector3(" Binormal", first.binormal); + ex::print_value(" Curvature", first.curvature); + ex::print_value(" Torsion", first.torsion); + + const auto& last = frames.back(); + std::cout << "\n Last frame (u = " << std::fixed << std::setprecision(2) + << u_values(n_samples - 1) << "):\n"; + ex::print_vector3(" Tangent", last.tangent); + ex::print_vector3(" Normal", last.normal); + ex::print_vector3(" Binormal", last.binormal); + ex::print_value(" Curvature", last.curvature); + ex::print_value(" Torsion", last.torsion); +} + +// --------------------------------------------------------------------------- +// 4. Frenet frames on circular trajectory +// --------------------------------------------------------------------------- + +static void frenet_frame_circular() { + ex::print_header("Frenet Frames -- Circular Trajectory"); + + const double r = 2.0; + const int n_samples = 8; + + std::cout << "Parameters: r = " << r << "\n"; + std::cout << "Circular curve: p(u) = (r*cos(u), r*sin(u), 0)\n\n"; + + // Build parameter values over one full circle + Eigen::VectorXd u_values(n_samples); + for (int i = 0; i < n_samples; ++i) { + u_values(i) = 2.0 * M_PI * static_cast(i) / (n_samples - 1); + } + + // Curve function + auto circle_fn = [r](double u) + -> std::tuple { + return circular_trajectory_with_derivatives(u, r); + }; + + const std::vector frames = compute_frenet_frames(circle_fn, u_values); + + // Print table + const int w = 12; + const int p = 4; + + std::cout << std::right + << std::setw(8) << "u" + << std::setw(w) << "T.x" + << std::setw(w) << "T.y" + << std::setw(w) << "T.z" + << std::setw(w) << "N.x" + << std::setw(w) << "N.y" + << std::setw(w) << "N.z" + << std::setw(w) << "curv" + << std::setw(w) << "torsion" + << "\n"; + ex::print_separator('-', 8 + 8 * w); + + for (int i = 0; i < n_samples; ++i) { + const auto& f = frames[static_cast(i)]; + std::cout << std::fixed << std::setprecision(p) + << std::setw(8) << u_values(i) + << std::setw(w) << f.tangent.x() + << std::setw(w) << f.tangent.y() + << std::setw(w) << f.tangent.z() + << std::setw(w) << f.normal.x() + << std::setw(w) << f.normal.y() + << std::setw(w) << f.normal.z() + << std::setw(w) << f.curvature + << std::setw(w) << f.torsion + << "\n"; + } + + // Verify expected properties of a circle + ex::print_separator('='); + std::cout << "Circular Frenet frame properties:\n"; + ex::print_value(" Expected curvature (1/r)", 1.0 / r); + ex::print_value(" Actual curvature (frame 0)", frames.front().curvature); + ex::print_value(" Expected torsion", 0.0); + ex::print_value(" Actual torsion (frame 0)", frames.front().torsion); + ex::print_vector3(" Binormal (should be +Z or -Z)", frames.front().binormal); +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main() { + std::cout << "Path and Frenet Frame Examples\n"; + ex::print_separator('='); + + linear_path_example(); + circular_path_example(); + frenet_frame_helicoidal(); + frenet_frame_circular(); + + std::cout << "\nAll path examples completed.\n"; + return 0; +} diff --git a/cpp/examples/polynomial_example.cpp b/cpp/examples/polynomial_example.cpp new file mode 100644 index 0000000..c692ec5 --- /dev/null +++ b/cpp/examples/polynomial_example.cpp @@ -0,0 +1,212 @@ +/// Polynomial trajectory example -- C++ port of examples/polynomials_ex.py +/// +/// Demonstrates polynomial trajectory generation with orders 3, 5, and 7: +/// 1. Order 3 two-point trajectory +/// 2. Order 5 two-point trajectory +/// 3. Order 7 two-point trajectory (comparison of all three) +/// 4. Multi-point trajectory with heuristic velocities + +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::motion; + +static void example_order3_two_point() { + ex::print_header("Example 1: Order 3 Two-Point Trajectory"); + + std::cout << " Boundary: pos 0->1, vel 0->0, t=[0, 2]\n\n"; + + BoundaryCondition bc_start{0.0, 0.0, 0.0, 0.0}; + BoundaryCondition bc_end{1.0, 0.0, 0.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + + PolynomialTrajectory traj(bc_start, bc_end, interval, PolynomialTrajectory::ORDER_3); + + ex::print_value("Order", static_cast(traj.order()), 0); + ex::print_value("Duration", traj.duration(), 2); + + // Show boundary values + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(2.0); + auto r_mid = traj.evaluate(1.0); + + std::cout << "\n Boundary check:\n"; + ex::print_value(" Start position", r_start.position, 4); + ex::print_value(" Start velocity", r_start.velocity, 4); + ex::print_value(" End position", r_end.position, 4); + ex::print_value(" End velocity", r_end.velocity, 4); + ex::print_value(" Mid position", r_mid.position, 4); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + interval.start, interval.end, 12); +} + +static void example_order5_two_point() { + ex::print_header("Example 2: Order 5 Two-Point Trajectory"); + + std::cout << " Boundary: pos 0->1, vel 0->0, acc 0->0, t=[0, 2]\n\n"; + + BoundaryCondition bc_start{0.0, 0.0, 0.0, 0.0}; + BoundaryCondition bc_end{1.0, 0.0, 0.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + + PolynomialTrajectory traj(bc_start, bc_end, interval, PolynomialTrajectory::ORDER_5); + + ex::print_value("Order", static_cast(traj.order()), 0); + ex::print_value("Duration", traj.duration(), 2); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(2.0); + + std::cout << "\n Boundary check:\n"; + ex::print_value(" Start position", r_start.position, 4); + ex::print_value(" Start velocity", r_start.velocity, 4); + ex::print_value(" Start acceleration", r_start.acceleration, 4); + ex::print_value(" End position", r_end.position, 4); + ex::print_value(" End velocity", r_end.velocity, 4); + ex::print_value(" End acceleration", r_end.acceleration, 4); + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + interval.start, interval.end, 12); +} + +static void example_order7_comparison() { + ex::print_header("Example 3: Order 7 Two-Point (Comparison of Orders 3, 5, 7)"); + + std::cout << " Boundary: pos 0->1, vel 0->0, acc 0->0, jerk 0->0, t=[0, 2]\n\n"; + + BoundaryCondition bc_start{0.0, 0.0, 0.0, 0.0}; + BoundaryCondition bc_end{1.0, 0.0, 0.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + + PolynomialTrajectory traj3(bc_start, bc_end, interval, PolynomialTrajectory::ORDER_3); + PolynomialTrajectory traj5(bc_start, bc_end, interval, PolynomialTrajectory::ORDER_5); + PolynomialTrajectory traj7(bc_start, bc_end, interval, PolynomialTrajectory::ORDER_7); + + // Compare at midpoint t=1.0 + auto r3 = traj3.evaluate(1.0); + auto r5 = traj5.evaluate(1.0); + auto r7 = traj7.evaluate(1.0); + + std::cout << " Values at midpoint (t=1.0):\n"; + std::cout << " Order 3 Order 5 Order 7\n"; + std::cout << std::fixed << std::setprecision(6); + std::cout << " Position: " << std::setw(11) << r3.position + << " " << std::setw(11) << r5.position + << " " << std::setw(11) << r7.position << "\n"; + std::cout << " Velocity: " << std::setw(11) << r3.velocity + << " " << std::setw(11) << r5.velocity + << " " << std::setw(11) << r7.velocity << "\n"; + std::cout << " Acceleration: " << std::setw(11) << r3.acceleration + << " " << std::setw(11) << r5.acceleration + << " " << std::setw(11) << r7.acceleration << "\n"; + std::cout << " Jerk: " << std::setw(11) << r3.jerk + << " " << std::setw(11) << r5.jerk + << " " << std::setw(11) << r7.jerk << "\n"; + + // Print coefficients + std::cout << "\n"; + ex::print_vector("Order 3 coefficients", traj3.coefficients()); + ex::print_vector("Order 5 coefficients", traj5.coefficients()); + ex::print_vector("Order 7 coefficients", traj7.coefficients()); + + // Full table for order 7 + std::cout << "\n Order 7 trajectory table:\n\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = traj7.evaluate(t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + interval.start, interval.end, 15); +} + +static void example_multipoint_heuristic() { + ex::print_header("Example 4: Multi-Point with Heuristic Velocities"); + + const std::vector points = {0.0, 0.5, 0.8, 1.0, 0.7, 0.3, 0.0}; + const std::vector times = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0}; + + std::cout << " Points: ["; + for (size_t i = 0; i < points.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << points[i]; + } + std::cout << "]\n"; + std::cout << " Times: ["; + for (size_t i = 0; i < times.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << times[i]; + } + std::cout << "]\n\n"; + + // Compute heuristic velocities + auto heur_vels = PolynomialTrajectory::heuristic_velocities(points, times); + std::cout << " Heuristic velocities: ["; + for (size_t i = 0; i < heur_vels.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(4) << heur_vels[i]; + } + std::cout << "]\n\n"; + + // Create multipoint trajectories at each order + const int orders[] = {PolynomialTrajectory::ORDER_3, + PolynomialTrajectory::ORDER_5, + PolynomialTrajectory::ORDER_7}; + const char* order_names[] = {"Order 3", "Order 5", "Order 7"}; + + for (int idx = 0; idx < 3; ++idx) { + auto segments = PolynomialTrajectory::multipoint_trajectory( + points, times, orders[idx]); + + std::cout << " --- " << order_names[idx] << " multi-point trajectory ---\n"; + ex::print_value(" Number of segments", + static_cast(segments.size()), 0); + + // Verify waypoint passage + std::cout << " Waypoint verification:\n"; + for (size_t i = 0; i < times.size(); ++i) { + auto r = PolynomialTrajectory::evaluate_multipoint(segments, times[i]); + std::cout << " t=" << std::fixed << std::setprecision(1) << times[i] + << ": pos=" << std::setprecision(4) << r.position + << " (expected " << points[i] << ")\n"; + } + + std::cout << "\n"; + ex::print_full_trajectory_table( + [&](double t) -> std::tuple { + auto r = PolynomialTrajectory::evaluate_multipoint(segments, t); + return {r.position, r.velocity, r.acceleration, r.jerk}; + }, + times.front(), times.back(), 18); + } +} + +int main() { + std::cout << "Polynomial Trajectory -- C++ Usage Examples\n"; + ex::print_separator('='); + + example_order3_two_point(); + example_order5_two_point(); + example_order7_comparison(); + example_multipoint_heuristic(); + + std::cout << "\nAll polynomial trajectory examples completed.\n"; + return 0; +} diff --git a/cpp/examples/quaternion_example.cpp b/cpp/examples/quaternion_example.cpp new file mode 100644 index 0000000..7de8900 --- /dev/null +++ b/cpp/examples/quaternion_example.cpp @@ -0,0 +1,429 @@ +/// Quaternion interpolation example โ€” C++ port of +/// examples/squad_c2_ex.py, examples/log_quat_new_ex.py, +/// examples/quat_visualization_ex.py +/// +/// Demonstrates quaternion construction, SLERP vs SQUAD comparison, +/// C2-continuous SQUAD, logarithmic quaternion interpolation (LQI), +/// modified LQI (mLQI), and a side-by-side method comparison. + +#include +#include +#include +#include +#include + +#include "example_utils.hpp" + +#include +#include +#include +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::quat; + +// --------------------------------------------------------------------------- +// Shared test data +// --------------------------------------------------------------------------- + +static constexpr double kRadToDeg = 180.0 / M_PI; + +/// Five waypoints from euler angles (degrees) at uniform time spacing. +static std::pair, std::vector> make_waypoints() { + // Euler angles in degrees: (roll, pitch, yaw) + // (0, 0, 0) (30, 45, 0) (60, -30, 90) (0, 45, 180) (-30, 0, 270) + const auto deg = [](double d) { return d * M_PI / 180.0; }; + + std::vector times = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector quats = { + Quaternion::from_euler_angles(deg(0), deg(0), deg(0)), + Quaternion::from_euler_angles(deg(30), deg(45), deg(0)), + Quaternion::from_euler_angles(deg(60), deg(-30), deg(90)), + Quaternion::from_euler_angles(deg(0), deg(45), deg(180)), + Quaternion::from_euler_angles(deg(-30), deg(0), deg(270)), + }; + return {times, quats}; +} + +// --------------------------------------------------------------------------- +// Helpers to format quaternion output +// --------------------------------------------------------------------------- + +/// Print a quaternion as "(w, x, y, z)". +static void print_quat(const std::string& label, const Quaternion& q, int precision = 4) { + std::cout << " " << label << ": (" + << std::fixed << std::setprecision(precision) + << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() + << ")\n"; +} + +/// Print a quaternion's euler angles as "(roll, pitch, yaw) deg". +static void print_euler_deg(const std::string& label, const Quaternion& q, int precision = 2) { + auto [roll, pitch, yaw] = q.to_euler_angles(); + std::cout << " " << label << ": (" + << std::fixed << std::setprecision(precision) + << roll * kRadToDeg << ", " + << pitch * kRadToDeg << ", " + << yaw * kRadToDeg << ") deg\n"; +} + +/// Print both forms on one logical block. +static void print_quat_full(const std::string& label, const Quaternion& q) { + print_quat(label, q); + print_euler_deg(label + " euler", q); +} + +// --------------------------------------------------------------------------- +// 1. Quaternion basics +// --------------------------------------------------------------------------- + +static void quaternion_basics() { + ex::print_header("1. Quaternion Basics"); + + // Construction methods + const Quaternion q_id = Quaternion::identity(); + print_quat_full("identity", q_id); + + const auto deg = [](double d) { return d * M_PI / 180.0; }; + const Quaternion q_euler = Quaternion::from_euler_angles(deg(30), deg(45), deg(0)); + print_quat_full("from_euler(30, 45, 0)", q_euler); + + const Quaternion q_aa = Quaternion::from_angle_axis( + M_PI / 4.0, Eigen::Vector3d::UnitZ()); + print_quat_full("from_angle_axis(pi/4, Z)", q_aa); + + const Quaternion q_raw(0.707, 0.0, 0.707, 0.0); + print_quat_full("Quaternion(0.707, 0, 0.707, 0)", q_raw); + + // Component access + ex::print_separator(); + std::cout << " Component access on q_euler:\n"; + ex::print_value("w", q_euler.w()); + ex::print_value("x", q_euler.x()); + ex::print_value("y", q_euler.y()); + ex::print_value("z", q_euler.z()); + ex::print_vector3("vec", q_euler.vec()); + ex::print_value("norm", q_euler.norm()); + + // Arithmetic (immutable) + ex::print_separator(); + std::cout << " Quaternion arithmetic:\n"; + const Quaternion q_product = q_euler * q_aa; + print_quat("q_euler * q_aa", q_product); + + const Quaternion q_conj = q_euler.conjugate(); + print_quat("q_euler.conjugate()", q_conj); + + const Quaternion q_inv = q_euler.inverse(); + print_quat("q_euler.inverse()", q_inv); + + const Quaternion q_unit = q_raw.unit(); + print_quat("q_raw.unit()", q_unit); + ex::print_value("q_raw.unit().norm()", q_unit.norm()); + + ex::print_value("q_euler.dot(q_aa)", q_euler.dot_product(q_aa)); + + // Exp/log/power + ex::print_separator(); + std::cout << " Exp / log / power:\n"; + const Quaternion q_log = Quaternion::log(q_euler); + print_quat("log(q_euler)", q_log); + const Quaternion q_exp = Quaternion::exp(q_log); + print_quat("exp(log(q_euler))", q_exp); + const Quaternion q_pow = Quaternion::power(q_euler, 0.5); + print_quat("power(q_euler, 0.5)", q_pow); + + // Conversions + ex::print_separator(); + std::cout << " Conversions:\n"; + const Eigen::Matrix3d rot = q_euler.to_rotation_matrix(); + ex::print_matrix("to_rotation_matrix()", rot); + + auto [axis, angle] = q_euler.to_axis_angle(); + ex::print_vector3("axis", axis); + ex::print_value("angle (deg)", angle * kRadToDeg, 2); +} + +// --------------------------------------------------------------------------- +// 2. SLERP vs SQUAD comparison +// --------------------------------------------------------------------------- + +static void slerp_squad_comparison() { + ex::print_header("2. SLERP vs SQUAD Comparison"); + + auto [times, quats] = make_waypoints(); + + const QuaternionSpline slerp_spline(times, quats, QuaternionSpline::Method::Slerp); + const QuaternionSpline squad_spline(times, quats, QuaternionSpline::Method::Squad); + + std::cout << " Waypoints (5 quaternions from euler angles):\n"; + for (size_t i = 0; i < quats.size(); ++i) { + print_euler_deg(" q[" + std::to_string(i) + "]", quats[i]); + } + + // Print euler angles at sample times + const int n_samples = 12; + const double t0 = times.front(); + const double t1 = times.back(); + + std::cout << "\n Interpolated euler angles (deg):\n\n"; + + const int tw = 10; + const int cw = 36; + std::cout << std::right + << std::setw(tw) << "Time" + << std::setw(cw) << "SLERP (roll, pitch, yaw)" + << std::setw(cw) << "SQUAD (roll, pitch, yaw)" << "\n"; + ex::print_separator('-', tw + 2 * cw); + + for (int i = 0; i <= n_samples; ++i) { + const double t = t0 + (t1 - t0) * static_cast(i) / n_samples; + + const Quaternion q_slerp = slerp_spline.evaluate(t); + const Quaternion q_squad = squad_spline.evaluate(t); + + auto [sr, sp, sy] = q_slerp.to_euler_angles(); + auto [qr, qp, qy] = q_squad.to_euler_angles(); + + std::cout << std::fixed << std::setprecision(3) + << std::setw(tw) << t + << " (" << std::setw(8) << sr * kRadToDeg + << ", " << std::setw(8) << sp * kRadToDeg + << ", " << std::setw(8) << sy * kRadToDeg << ")" + << " (" << std::setw(8) << qr * kRadToDeg + << ", " << std::setw(8) << qp * kRadToDeg + << ", " << std::setw(8) << qy * kRadToDeg << ")" + << "\n"; + } + + std::cout << "\n Key: SLERP is C0 (linear per segment); SQUAD is C1 (smoother).\n"; +} + +// --------------------------------------------------------------------------- +// 3. SquadC2 โ€” C2-continuous +// --------------------------------------------------------------------------- + +static void squad_c2_example() { + ex::print_header("3. SQUAD C2 โ€” C2-Continuous Interpolation"); + + auto [times, quats] = make_waypoints(); + const SquadC2 spline(times, quats); + + std::cout << " SquadC2 constructed with " << quats.size() << " waypoints\n"; + std::cout << " Time range: [" << spline.t_min() << ", " << spline.t_max() << "]\n\n"; + + // Print velocity and acceleration norms to show C2 continuity + const int n_samples = 16; + const double t0 = spline.t_min(); + const double t1 = spline.t_max(); + + const int tw = 10; + const int cw = 18; + std::cout << std::right + << std::setw(tw) << "Time" + << std::setw(cw) << "||vel||" + << std::setw(cw) << "||accel||" + << "\n"; + ex::print_separator('-', tw + 2 * cw); + + for (int i = 0; i <= n_samples; ++i) { + const double t = t0 + (t1 - t0) * static_cast(i) / n_samples; + const Eigen::Vector3d vel = spline.evaluate_velocity(t); + const Eigen::Vector3d acc = spline.evaluate_acceleration(t); + + std::cout << std::fixed << std::setprecision(4) + << std::setw(tw) << t + << std::setw(cw) << vel.norm() + << std::setw(cw) << acc.norm() + << "\n"; + } + + std::cout << "\n Note: zero-clamped boundaries => vel norms are zero at t=0 and t=4.\n" + << " Smooth (continuous) velocity and acceleration confirm C2 continuity.\n"; +} + +// --------------------------------------------------------------------------- +// 4. Log-quaternion interpolation (LQI) +// --------------------------------------------------------------------------- + +static void log_quaternion_example() { + ex::print_header("4. Logarithmic Quaternion Interpolation (LQI)"); + + auto [times, quats] = make_waypoints(); + const LogQuaternionInterpolation lqi(times, quats, /*degree=*/3); + + std::cout << " LQI constructed with degree 3, " + << quats.size() << " waypoints\n\n"; + + const int n_samples = 12; + const double t0 = lqi.t_min(); + const double t1 = lqi.t_max(); + + const int tw = 10; + const int ew = 36; + const int vw = 14; + std::cout << std::right + << std::setw(tw) << "Time" + << std::setw(ew) << "Euler (roll, pitch, yaw) deg" + << std::setw(vw) << "||vel||" + << "\n"; + ex::print_separator('-', tw + ew + vw); + + for (int i = 0; i <= n_samples; ++i) { + const double t = t0 + (t1 - t0) * static_cast(i) / n_samples; + const Quaternion q = lqi.evaluate(t); + const Eigen::Vector3d vel = lqi.evaluate_velocity(t); + auto [r, p, y] = q.to_euler_angles(); + + std::cout << std::fixed << std::setprecision(3) + << std::setw(tw) << t + << " (" << std::setw(8) << r * kRadToDeg + << ", " << std::setw(8) << p * kRadToDeg + << ", " << std::setw(8) << y * kRadToDeg << ")" + << std::setprecision(4) + << std::setw(vw) << vel.norm() + << "\n"; + } + + std::cout << "\n LQI uses axis-angle representation with B-spline interpolation.\n" + << " Provides C2 continuous trajectories in the tangent space.\n"; +} + +// --------------------------------------------------------------------------- +// 5. Modified log-quaternion interpolation (mLQI) +// --------------------------------------------------------------------------- + +static void modified_log_quaternion_example() { + ex::print_header("5. Modified Log-Quaternion Interpolation (mLQI)"); + + auto [times, quats] = make_waypoints(); + const ModifiedLogQuaternionInterpolation mlqi( + times, quats, /*degree=*/3, /*normalize_axis=*/true); + + std::cout << " mLQI constructed with degree 3, normalize_axis=true, " + << quats.size() << " waypoints\n\n"; + + const int n_samples = 12; + const double t0 = mlqi.t_min(); + const double t1 = mlqi.t_max(); + + const int tw = 10; + const int ew = 36; + const int vw = 14; + std::cout << std::right + << std::setw(tw) << "Time" + << std::setw(ew) << "Euler (roll, pitch, yaw) deg" + << std::setw(vw) << "||vel||" + << "\n"; + ex::print_separator('-', tw + ew + vw); + + for (int i = 0; i <= n_samples; ++i) { + const double t = t0 + (t1 - t0) * static_cast(i) / n_samples; + const Quaternion q = mlqi.evaluate(t); + const Eigen::Vector4d vel = mlqi.evaluate_velocity(t); + auto [r, p, y] = q.to_euler_angles(); + + std::cout << std::fixed << std::setprecision(3) + << std::setw(tw) << t + << " (" << std::setw(8) << r * kRadToDeg + << ", " << std::setw(8) << p * kRadToDeg + << ", " << std::setw(8) << y * kRadToDeg << ")" + << std::setprecision(4) + << std::setw(vw) << vel.norm() + << "\n"; + } + + std::cout << "\n mLQI decouples angle (theta) from unit axis (X, Y, Z).\n" + << " Provides improved numerical stability for complex trajectories.\n"; +} + +// --------------------------------------------------------------------------- +// 6. Side-by-side method comparison +// --------------------------------------------------------------------------- + +static void method_comparison() { + ex::print_header("6. Method Comparison โ€” All 4 Methods"); + + auto [times, quats] = make_waypoints(); + + const QuaternionSpline slerp_sp(times, quats, QuaternionSpline::Method::Slerp); + const QuaternionSpline squad_sp(times, quats, QuaternionSpline::Method::Squad); + const SquadC2 sc2(times, quats); + const LogQuaternionInterpolation lqi(times, quats, 3); + + const int n_samples = 10; + const double t0 = times.front(); + const double t1 = times.back(); + + // Table header + const int tw = 8; + const int mw = 30; + std::cout << std::right + << std::setw(tw) << "Time" + << std::setw(mw) << "SLERP euler (deg)" + << std::setw(mw) << "SQUAD euler (deg)" + << std::setw(mw) << "SQUAD-C2 euler (deg)" + << std::setw(mw) << "LQI euler (deg)" + << "\n"; + ex::print_separator('-', tw + 4 * mw); + + for (int i = 0; i <= n_samples; ++i) { + const double t = t0 + (t1 - t0) * static_cast(i) / n_samples; + + const auto fmt = [&](const Quaternion& q) -> std::string { + auto [r, p, y] = q.to_euler_angles(); + std::ostringstream oss; + oss << std::fixed << std::setprecision(1) + << "(" << std::setw(6) << r * kRadToDeg + << ", " << std::setw(6) << p * kRadToDeg + << ", " << std::setw(6) << y * kRadToDeg << ")"; + return oss.str(); + }; + + const Quaternion q_sl = slerp_sp.evaluate(t); + const Quaternion q_sq = squad_sp.evaluate(t); + const Quaternion q_c2 = sc2.evaluate(t); + const Quaternion q_lq = lqi.evaluate(t); + + std::cout << std::fixed << std::setprecision(2) + << std::setw(tw) << t + << std::setw(mw) << fmt(q_sl) + << std::setw(mw) << fmt(q_sq) + << std::setw(mw) << fmt(q_c2) + << std::setw(mw) << fmt(q_lq) + << "\n"; + } + + std::cout << "\n Summary of continuity guarantees:\n" + << " SLERP โ€” C0 (piecewise linear on S3)\n" + << " SQUAD โ€” C1 (spherical cubic, smooth velocity)\n" + << " SQUAD-C2 โ€” C2 (quintic, smooth velocity + acceleration)\n" + << " LQI โ€” C2 (B-spline in log space)\n"; +} + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main() { + std::cout << "Quaternion Interpolation Examples\n"; + ex::print_separator('=', 60); + std::cout << "Demonstrates quaternion basics, SLERP/SQUAD comparison,\n" + << "C2-continuous SQUAD, logarithmic interpolation (LQI),\n" + << "modified LQI (mLQI), and side-by-side method comparison.\n"; + + quaternion_basics(); + slerp_squad_comparison(); + squad_c2_example(); + log_quaternion_example(); + modified_log_quaternion_example(); + method_comparison(); + + std::cout << "\n"; + ex::print_separator('=', 60); + std::cout << "All quaternion interpolation examples completed.\n"; + + return 0; +} diff --git a/cpp/examples/shared/example_utils.hpp b/cpp/examples/shared/example_utils.hpp new file mode 100644 index 0000000..b54b7f0 --- /dev/null +++ b/cpp/examples/shared/example_utils.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace interpolatecpp::examples { + +inline void print_header(const std::string& title) { + std::cout << "\n=== " << title << " ===\n\n"; +} + +inline void print_separator(char ch = '-', int width = 60) { + std::cout << std::string(static_cast(width), ch) << "\n"; +} + +inline void print_value(const std::string& label, double value, int precision = 6) { + std::cout << " " << label << ": " << std::fixed << std::setprecision(precision) << value + << "\n"; +} + +inline void print_vector(const std::string& label, const Eigen::VectorXd& v, int precision = 4) { + std::cout << " " << label << ": ["; + for (Eigen::Index i = 0; i < v.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(precision) << v(i); + } + std::cout << "]\n"; +} + +inline void print_vector3(const std::string& label, const Eigen::Vector3d& v, + int precision = 4) { + std::cout << " " << label << ": (" << std::fixed << std::setprecision(precision) << v.x() + << ", " << v.y() << ", " << v.z() << ")\n"; +} + +/// Print a trajectory table with columns: Time | Position | Velocity | Acceleration. +inline void print_trajectory_table( + const std::function& pos_fn, + const std::function& vel_fn, + const std::function& acc_fn, double t_start, double t_end, + int num_samples = 15) { + const int w = 14; + const int p = 6; + + std::cout << std::right << std::setw(w) << "Time" << std::setw(w) << "Position" + << std::setw(w) << "Velocity" << std::setw(w) << "Acceleration" + << "\n"; + print_separator('-', 4 * w); + + for (int i = 0; i <= num_samples; ++i) { + double t = t_start + (t_end - t_start) * static_cast(i) / num_samples; + std::cout << std::fixed << std::setprecision(p) << std::setw(w) << t << std::setw(w) + << pos_fn(t) << std::setw(w) << vel_fn(t) << std::setw(w) << acc_fn(t) + << "\n"; + } + std::cout << "\n"; +} + +/// Print trajectory table with jerk (4 columns + jerk). +inline void print_full_trajectory_table( + const std::function(double)>& eval_fn, + double t_start, double t_end, int num_samples = 15) { + const int w = 14; + const int p = 6; + + std::cout << std::right << std::setw(w) << "Time" << std::setw(w) << "Position" + << std::setw(w) << "Velocity" << std::setw(w) << "Acceleration" << std::setw(w) + << "Jerk" + << "\n"; + print_separator('-', 5 * w); + + for (int i = 0; i <= num_samples; ++i) { + double t = t_start + (t_end - t_start) * static_cast(i) / num_samples; + auto [pos, vel, acc, jrk] = eval_fn(t); + std::cout << std::fixed << std::setprecision(p) << std::setw(w) << t << std::setw(w) + << pos << std::setw(w) << vel << std::setw(w) << acc << std::setw(w) << jrk + << "\n"; + } + std::cout << "\n"; +} + +/// Print a matrix row by row. +inline void print_matrix(const std::string& label, const Eigen::MatrixXd& m, int precision = 4) { + std::cout << " " << label << " (" << m.rows() << "x" << m.cols() << "):\n"; + for (Eigen::Index i = 0; i < m.rows(); ++i) { + std::cout << " ["; + for (Eigen::Index j = 0; j < m.cols(); ++j) { + if (j > 0) std::cout << ", "; + std::cout << std::fixed << std::setprecision(precision) << std::setw(10) << m(i, j); + } + std::cout << "]\n"; + } +} + +} // namespace interpolatecpp::examples diff --git a/cpp/examples/trapezoidal_example.cpp b/cpp/examples/trapezoidal_example.cpp new file mode 100644 index 0000000..ec43de6 --- /dev/null +++ b/cpp/examples/trapezoidal_example.cpp @@ -0,0 +1,218 @@ +/// Trapezoidal trajectory example -- C++ port of examples/trapezoidal_ex.py +/// +/// Demonstrates trapezoidal velocity profile trajectory generation: +/// 1. Basic trajectory (q0=0, q1=10, amax=2, vmax=3) +/// 2. Non-zero boundary velocities +/// 3. Duration-based construction +/// 4. Multi-point trajectory with heuristic velocities +/// 5. Time-constrained waypoints + +#include + +#include "example_utils.hpp" + +#include +#include +#include + +namespace ex = interpolatecpp::examples; +using namespace interpolatecpp::motion; + +static void example_basic_trajectory() { + ex::print_header("Example 1: Basic Trapezoidal Trajectory"); + + std::cout << " q0=0, q1=10, amax=3, vmax=4, v0=0, v1=0\n\n"; + + TrapezoidalTrajectory traj(0.0, 10.0, 3.0, 4.0); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 2); + + auto r_start = traj.evaluate(0.0); + auto r_mid = traj.evaluate(dur / 2.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Position at t=0", r_start.position, 2); + ex::print_value("Position at t=mid", r_mid.position, 2); + ex::print_value("Position at t=end", r_end.position, 2); + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double t) { return traj.evaluate(t).position; }, + [&](double t) { return traj.evaluate(t).velocity; }, + [&](double t) { return traj.evaluate(t).acceleration; }, + traj.t_start(), traj.t_end(), 15); +} + +static void example_nonzero_velocities() { + ex::print_header("Example 2: Non-Zero Boundary Velocities"); + + std::cout << " q0=0, q1=10, amax=5, vmax=4, v0=1.5, v1=2.0\n\n"; + + TrapezoidalTrajectory traj(0.0, 10.0, 5.0, 4.0, 1.5, 2.0); + + const double dur = traj.duration(); + ex::print_value("Duration", dur, 2); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Initial velocity", r_start.velocity, 2); + ex::print_value("Final velocity", r_end.velocity, 2); + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double t) { return traj.evaluate(t).position; }, + [&](double t) { return traj.evaluate(t).velocity; }, + [&](double t) { return traj.evaluate(t).acceleration; }, + traj.t_start(), traj.t_end(), 15); +} + +static void example_duration_based() { + ex::print_header("Example 3: Duration-Based Construction"); + + std::cout << " q0=0, q1=10, amax=5, v0=0, v1=0, duration=4.0\n\n"; + + TrapezoidalTrajectory traj( + TrapezoidalTrajectory::DurationBased{}, + 0.0, 10.0, 5.0, 0.0, 0.0, 0.0, 4.0); + + const double dur = traj.duration(); + ex::print_value("Specified duration", 4.0, 2); + ex::print_value("Actual duration", dur, 2); + + auto r_start = traj.evaluate(0.0); + auto r_end = traj.evaluate(dur); + + ex::print_value("Start position", r_start.position, 4); + ex::print_value("End position", r_end.position, 4); + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double t) { return traj.evaluate(t).position; }, + [&](double t) { return traj.evaluate(t).velocity; }, + [&](double t) { return traj.evaluate(t).acceleration; }, + traj.t_start(), traj.t_end(), 15); +} + +static void example_multipoint_heuristic() { + ex::print_header("Example 4: Multi-Point Trajectory with Heuristic Velocities"); + + const std::vector points = {0.0, 5.0, -3.0, 8.0, -2.0, 4.0, 0.0}; + const double amax = 8.0; + const double vmax = 6.0; + + std::cout << " Waypoints: ["; + for (size_t i = 0; i < points.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << points[i]; + } + std::cout << "]\n"; + std::cout << " amax=" << amax << ", vmax=" << vmax << "\n\n"; + + auto segments = TrapezoidalTrajectory::interpolate_waypoints( + points, amax, vmax); + + // Compute total duration from segments + double total_dur = 0.0; + if (!segments.empty()) { + total_dur = segments.back().t_end() - segments.front().t_start(); + } + ex::print_value("Total duration", total_dur, 2); + ex::print_value("Number of segments", static_cast(segments.size()), 0); + + // Verify waypoint passage + std::cout << "\n Verifying waypoint positions:\n"; + for (size_t i = 0; i < segments.size(); ++i) { + auto r = TrapezoidalTrajectory::evaluate_multipoint( + segments, segments[i].t_start()); + std::cout << " Segment " << i << " start: position = " + << std::fixed << std::setprecision(2) << r.position << "\n"; + } + { + auto r_final = TrapezoidalTrajectory::evaluate_multipoint( + segments, segments.back().t_end()); + std::cout << " Final: position = " + << std::fixed << std::setprecision(2) << r_final.position << "\n"; + } + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).position; + }, + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).velocity; + }, + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).acceleration; + }, + segments.front().t_start(), segments.back().t_end(), 20); +} + +static void example_time_constrained() { + ex::print_header("Example 5: Time-Constrained Multi-Point Trajectory"); + + const std::vector points = {0.0, 5.0, 10.0, 7.0, 15.0}; + const std::vector times = {0.0, 2.0, 4.0, 7.0, 10.0}; + const double amax = 15.0; + const double vmax = 100.0; // Large vmax to allow time-based planning + + std::cout << " Waypoints: ["; + for (size_t i = 0; i < points.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << points[i]; + } + std::cout << "]\n"; + std::cout << " Times: ["; + for (size_t i = 0; i < times.size(); ++i) { + if (i > 0) std::cout << ", "; + std::cout << times[i]; + } + std::cout << "]\n\n"; + + auto segments = TrapezoidalTrajectory::interpolate_waypoints( + points, amax, vmax, 0.0, 0.0, times); + + double total_dur = 0.0; + if (!segments.empty()) { + total_dur = segments.back().t_end() - segments.front().t_start(); + } + ex::print_value("Total duration", total_dur, 2); + + // Verify position at specified times + std::cout << "\n Verifying position at specified times:\n"; + for (size_t i = 0; i < times.size(); ++i) { + auto r = TrapezoidalTrajectory::evaluate_multipoint(segments, times[i]); + std::cout << " At t=" << std::fixed << std::setprecision(1) << times[i] + << "s: Expected=" << std::setprecision(1) << points[i] + << ", Actual=" << std::setprecision(1) << r.position << "\n"; + } + + std::cout << "\n"; + ex::print_trajectory_table( + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).position; + }, + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).velocity; + }, + [&](double t) { + return TrapezoidalTrajectory::evaluate_multipoint(segments, t).acceleration; + }, + times.front(), times.back(), 20); +} + +int main() { + std::cout << "TrapezoidalTrajectory -- C++ Usage Examples\n"; + ex::print_separator('='); + + example_basic_trajectory(); + example_nonzero_velocities(); + example_duration_based(); + example_multipoint_heuristic(); + example_time_constrained(); + + std::cout << "\nAll trapezoidal trajectory examples completed.\n"; + return 0; +} diff --git a/cpp/include/interpolatecpp/bspline/approximation_bspline.hpp b/cpp/include/interpolatecpp/bspline/approximation_bspline.hpp new file mode 100644 index 0000000..4011c91 --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/approximation_bspline.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#include +#include +#include + +namespace interpolatecpp::bspline { + +/// Least-squares B-spline approximation of data points. +/// +/// Fits a B-spline with fewer control points than data points, +/// exactly interpolating the endpoints and approximating internal points. +/// Follows "The NURBS Book" Section 8.5. +class INTERPOLATECPP_API ApproximationBSpline : public BSpline { + public: + /// Construct an approximation B-spline. + /// + /// @param points Points to approximate (n x d matrix) + /// @param num_control_points Number of control points to use + /// @param degree Degree (default 3 for cubic) + /// @param weights Weights for internal points (nullopt for uniform) + /// @param method Parameterization method + /// @param debug Print debug information + ApproximationBSpline(const Eigen::MatrixXd& points, int num_control_points, + int degree = 3, + const std::optional& weights = std::nullopt, + Parameterization method = Parameterization::ChordLength, + bool debug = false); + + /// Calculate sum of squared approximation errors. + [[nodiscard]] double calculate_approximation_error() const; + + // Accessors + [[nodiscard]] const Eigen::MatrixXd& original_points() const noexcept { return original_points_; } + [[nodiscard]] const Eigen::VectorXd& original_parameters() const noexcept { + return original_parameters_; + } + + private: + Eigen::MatrixXd original_points_; + Eigen::VectorXd original_parameters_; + bool debug_; + + [[nodiscard]] static Eigen::VectorXd compute_parameters( + const Eigen::MatrixXd& points, Parameterization method); + [[nodiscard]] static Eigen::VectorXd compute_knots(int degree, int num_control_points, + int num_points, + const Eigen::VectorXd& u_bar); + [[nodiscard]] Eigen::MatrixXd approximate_control_points( + const Eigen::MatrixXd& points, int degree, const Eigen::VectorXd& knots, + const Eigen::VectorXd& u_bar, int num_control_points, + const Eigen::VectorXd& weights) const; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/bspline/bspline.hpp b/cpp/include/interpolatecpp/bspline/bspline.hpp new file mode 100644 index 0000000..e65eb0f --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/bspline.hpp @@ -0,0 +1,77 @@ +#pragma once + +#include +#include + +#include + +namespace interpolatecpp::bspline { + +/// B-spline curve of arbitrary degree supporting N-dimensional control points. +/// +/// Implements the Cox-de Boor algorithm for evaluation and derivative computation. +/// Control points are stored as rows of a matrix (n_points x dimension). +class INTERPOLATECPP_API BSpline { + public: + /// Construct a B-spline curve. + /// + /// @param degree Degree of the B-spline (must be non-negative) + /// @param knots Non-decreasing knot vector + /// @param control_points Control points (n_points x dimension matrix, or vector for 1D) + /// @throws std::invalid_argument if inputs violate B-spline requirements + BSpline(int degree, std::span knots, const Eigen::MatrixXd& control_points); + + virtual ~BSpline() = default; + + // Evaluation (satisfies CurveEvaluator concept) + [[nodiscard]] Eigen::VectorXd evaluate(double u) const; + [[nodiscard]] Eigen::VectorXd evaluate_derivative(double u, int order = 1) const; + + // Curve generation + [[nodiscard]] std::pair + generate_curve_points(int num_points = 100) const; + + // Knot span and basis functions + [[nodiscard]] int find_knot_span(double u) const; + [[nodiscard]] Eigen::VectorXd basis_functions(double u, int span_index) const; + [[nodiscard]] Eigen::MatrixXd basis_function_derivatives(double u, int span_index, + int order) const; + + // Static knot vector generators + [[nodiscard]] static Eigen::VectorXd create_uniform_knots(int degree, + int num_control_points, + double domain_min = 0.0, + double domain_max = 1.0); + + [[nodiscard]] static Eigen::VectorXd create_periodic_knots(int degree, + int num_control_points, + double domain_min = 0.0, + double domain_max = 1.0); + + // Accessors + [[nodiscard]] int degree() const noexcept { return degree_; } + [[nodiscard]] const Eigen::VectorXd& knots() const noexcept { return knots_; } + [[nodiscard]] const Eigen::MatrixXd& control_points() const noexcept { return control_points_; } + [[nodiscard]] double u_min() const noexcept { return u_min_; } + [[nodiscard]] double u_max() const noexcept { return u_max_; } + [[nodiscard]] int dimension() const noexcept { return dimension_; } + [[nodiscard]] int n_control_points() const noexcept { + return static_cast(control_points_.rows()); + } + + protected: + /// Protected constructor for derived classes that will initialize fields themselves. + struct DeferInit {}; + explicit BSpline(DeferInit) : degree_(0), u_min_(0), u_max_(0), dimension_(0) {} + + int degree_; + Eigen::VectorXd knots_; + Eigen::MatrixXd control_points_; // (n_points x dimension) + double u_min_; + double u_max_; + int dimension_; + + static constexpr double kEps = 1e-10; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/bspline/bspline_interpolator.hpp b/cpp/include/interpolatecpp/bspline/bspline_interpolator.hpp new file mode 100644 index 0000000..bbaac84 --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/bspline_interpolator.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +#include +#include + +namespace interpolatecpp::bspline { + +/// B-spline interpolator supporting degrees 3, 4, and 5. +/// +/// Computes knot vector and control points to interpolate given data points +/// with appropriate continuity (C2 for degree 3, C3 for degree 4, C4 for degree 5). +class INTERPOLATECPP_API BSplineInterpolator : public BSpline { + public: + /// Construct a B-spline interpolator. + /// + /// @param degree Degree (3, 4, or 5) + /// @param points Points to interpolate (n x d matrix) + /// @param times Time instants (nullopt for uniform spacing) + /// @param initial_velocity Initial velocity constraint + /// @param final_velocity Final velocity constraint + /// @param initial_acceleration Initial acceleration constraint + /// @param final_acceleration Final acceleration constraint + /// @param cyclic Use cyclic (periodic) conditions + BSplineInterpolator( + int degree, const Eigen::MatrixXd& points, + const std::optional& times = std::nullopt, + const std::optional& initial_velocity = std::nullopt, + const std::optional& final_velocity = std::nullopt, + const std::optional& initial_acceleration = std::nullopt, + const std::optional& final_acceleration = std::nullopt, + bool cyclic = false); + + // Accessors + [[nodiscard]] const Eigen::MatrixXd& interp_points() const noexcept { return interp_points_; } + [[nodiscard]] const Eigen::VectorXd& times() const noexcept { return times_; } + + private: + Eigen::MatrixXd interp_points_; + Eigen::VectorXd times_; + std::optional initial_velocity_; + std::optional final_velocity_; + std::optional initial_acceleration_; + std::optional final_acceleration_; + bool cyclic_; + + static constexpr double kConditionThreshold = 1e12; + static constexpr double kRegularizationEps = 1e-10; + + [[nodiscard]] static Eigen::VectorXd create_knot_vector(int degree, + const Eigen::MatrixXd& points, + const Eigen::VectorXd& times); + [[nodiscard]] Eigen::MatrixXd compute_control_points(int degree, + const Eigen::MatrixXd& points, + const Eigen::VectorXd& times, + const Eigen::VectorXd& knots) const; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/bspline/bspline_parameters.hpp b/cpp/include/interpolatecpp/bspline/bspline_parameters.hpp new file mode 100644 index 0000000..565d1ed --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/bspline_parameters.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include + +namespace interpolatecpp::bspline { + +/// Parameterization method for computing parameter values from data points. +enum class Parameterization { + EquallySpaced, + ChordLength, + Centripetal, +}; + +/// Configuration parameters for SmoothingCubicBSpline. +struct BSplineParams { + double mu = 0.5; + std::optional weights = std::nullopt; + std::optional v0 = std::nullopt; + std::optional vn = std::nullopt; + Parameterization method = Parameterization::ChordLength; + bool enforce_endpoints = false; + bool auto_derivatives = false; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/bspline/cubic_bspline_interpolation.hpp b/cpp/include/interpolatecpp/bspline/cubic_bspline_interpolation.hpp new file mode 100644 index 0000000..591a973 --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/cubic_bspline_interpolation.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace interpolatecpp::bspline { + +/// Cubic B-spline interpolation through a set of points. +/// +/// Creates a degree-3 B-spline that passes through all specified points +/// with C2 continuity. Follows "The NURBS Book" Section 8.4.2. +class INTERPOLATECPP_API CubicBSplineInterpolation : public BSpline { + public: + /// Construct a cubic B-spline interpolation. + /// + /// @param points Points to interpolate (n x d matrix) + /// @param v0 Initial endpoint derivative (nullopt for zero or auto) + /// @param vn Final endpoint derivative (nullopt for zero or auto) + /// @param method Parameterization method + /// @param auto_derivatives Whether to auto-calculate derivatives + CubicBSplineInterpolation( + const Eigen::MatrixXd& points, + const std::optional& v0 = std::nullopt, + const std::optional& vn = std::nullopt, + Parameterization method = Parameterization::ChordLength, + bool auto_derivatives = false); + + // Accessors + [[nodiscard]] const Eigen::MatrixXd& interpolation_points() const noexcept { + return interpolation_points_; + } + [[nodiscard]] const Eigen::VectorXd& u_bars() const noexcept { return u_bars_; } + [[nodiscard]] const Eigen::VectorXd& start_derivative() const noexcept { return v0_; } + [[nodiscard]] const Eigen::VectorXd& end_derivative() const noexcept { return vn_; } + + private: + Eigen::MatrixXd interpolation_points_; + Eigen::VectorXd u_bars_; + Eigen::VectorXd v0_; + Eigen::VectorXd vn_; + + static constexpr double kParamDiffThreshold = 1e-10; + static constexpr int kMinPointsForTridiagonal = 2; + + [[nodiscard]] static Eigen::VectorXd calculate_parameters( + const Eigen::MatrixXd& points, Parameterization method); + [[nodiscard]] static Eigen::VectorXd calculate_knot_vector(const Eigen::VectorXd& u_bars, + int n_points); + [[nodiscard]] Eigen::MatrixXd calculate_control_points(const Eigen::VectorXd& knots) const; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/bspline/smoothing_cubic_bspline.hpp b/cpp/include/interpolatecpp/bspline/smoothing_cubic_bspline.hpp new file mode 100644 index 0000000..66feaf0 --- /dev/null +++ b/cpp/include/interpolatecpp/bspline/smoothing_cubic_bspline.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include +#include +#include + +namespace interpolatecpp::bspline { + +/// Smoothing cubic B-spline approximation. +/// +/// Creates a cubic B-spline that balances fitting accuracy with smoothness, +/// controlled by the smoothing parameter mu in [0, 1]. +/// Implements the Tikhonov regularization: (B^T W B + lambda C^T A C) P = B^T W Q +/// as described in "The NURBS Book" Section 8.7. +class INTERPOLATECPP_API SmoothingCubicBSpline : public BSpline { + public: + /// Construct a smoothing cubic B-spline. + /// + /// @param points Points to approximate (n x d matrix) + /// @param params Configuration parameters (nullopt for defaults) + SmoothingCubicBSpline(const Eigen::MatrixXd& points, + const BSplineParams& params = BSplineParams{}); + + /// Calculate per-point approximation errors. + [[nodiscard]] Eigen::VectorXd calculate_approximation_error() const; + + /// Calculate total weighted approximation error. + [[nodiscard]] double calculate_total_error() const; + + // Accessors + [[nodiscard]] const Eigen::MatrixXd& approximation_points() const noexcept { + return approximation_points_; + } + [[nodiscard]] const Eigen::VectorXd& u_bars() const noexcept { return u_bars_; } + [[nodiscard]] double mu() const noexcept { return mu_; } + [[nodiscard]] double lambda_param() const noexcept { return lambda_param_; } + + private: + Eigen::MatrixXd approximation_points_; + Eigen::VectorXd u_bars_; + Eigen::VectorXd weights_; + double mu_; + double lambda_param_; + bool enforce_endpoints_; + Eigen::VectorXd v0_; + Eigen::VectorXd vn_; + + static constexpr double kEpsilon = 1e-10; + + [[nodiscard]] static Eigen::VectorXd calculate_parameters( + const Eigen::MatrixXd& points, Parameterization method); + [[nodiscard]] Eigen::VectorXd calculate_knot_vector() const; + [[nodiscard]] Eigen::MatrixXd construct_b_matrix() const; + [[nodiscard]] Eigen::MatrixXd construct_a_matrix() const; + [[nodiscard]] Eigen::MatrixXd construct_c_matrix() const; + [[nodiscard]] Eigen::MatrixXd calculate_control_points_impl() const; + [[nodiscard]] Eigen::MatrixXd calculate_control_points_with_endpoints() const; +}; + +} // namespace interpolatecpp::bspline diff --git a/cpp/include/interpolatecpp/concepts.hpp b/cpp/include/interpolatecpp/concepts.hpp new file mode 100644 index 0000000..32f8123 --- /dev/null +++ b/cpp/include/interpolatecpp/concepts.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include + +namespace interpolatecpp { + +/// Concept for scalar (1D) trajectory evaluation. +/// Conforming types: CubicSpline, CubicSmoothingSpline, +/// CubicSplineWithAcceleration1, CubicSplineWithAcceleration2 +template +concept ScalarTrajectory = requires(const T& traj, double t, const Eigen::VectorXd& tv) { + { traj.evaluate(t) } -> std::convertible_to; + { traj.evaluate(tv) } -> std::convertible_to; + { traj.evaluate_velocity(t) } -> std::convertible_to; + { traj.evaluate_velocity(tv) } -> std::convertible_to; + { traj.evaluate_acceleration(t) } -> std::convertible_to; + { traj.evaluate_acceleration(tv) } -> std::convertible_to; +}; + +/// Concept for parametric curve evaluation with derivative support. +/// Conforming types: BSpline family (Phase 2) +template +concept CurveEvaluator = requires(const T& curve, double u, int order) { + { curve.evaluate(u) } -> std::convertible_to; + { curve.evaluate_derivative(u, order) } -> std::convertible_to; +}; + +/// Concept for 3D geometric path evaluation by arc length. +/// Conforming types: LinearPath, CircularPath (Phase 5) +template +concept GeometricPath = requires(const T& path, double s, const Eigen::VectorXd& sv) { + { path.position(s) } -> std::convertible_to; + { path.position(sv) } -> std::convertible_to; + { path.velocity(s) } -> std::convertible_to; + { path.acceleration(s) } -> std::convertible_to; +}; + +/// Concept for quaternion-valued trajectory evaluation. +/// Conforming types: SquadC2, LogQuaternionInterpolation (Phase 4) +template +concept QuaternionTrajectory = requires(const T& traj, double t) { + { traj.evaluate(t) } -> std::convertible_to; + { traj.evaluate_velocity(t) } -> std::convertible_to; + { traj.evaluate_acceleration(t) } -> std::convertible_to; +}; + +/// Concept for callable trajectory functions returning (pos, vel, acc). +/// Conforming types: Motion profile callables (Phase 3) +template +concept TrajectoryFunction = requires(const T& func, double t) { + { func(t) } -> std::convertible_to>; +}; + +} // namespace interpolatecpp diff --git a/cpp/include/interpolatecpp/config.hpp b/cpp/include/interpolatecpp/config.hpp new file mode 100644 index 0000000..56d7253 --- /dev/null +++ b/cpp/include/interpolatecpp/config.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +#if defined(_WIN32) || defined(__CYGWIN__) + #ifdef INTERPOLATECPP_EXPORTS + #define INTERPOLATECPP_API __declspec(dllexport) + #else + #define INTERPOLATECPP_API __declspec(dllimport) + #endif +#elif defined(__GNUC__) || defined(__clang__) + #define INTERPOLATECPP_API __attribute__((visibility("default"))) +#else + #define INTERPOLATECPP_API +#endif diff --git a/cpp/include/interpolatecpp/motion/double_s_trajectory.hpp b/cpp/include/interpolatecpp/motion/double_s_trajectory.hpp new file mode 100644 index 0000000..4eccc59 --- /dev/null +++ b/cpp/include/interpolatecpp/motion/double_s_trajectory.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include +#include + +#include +#include + +namespace interpolatecpp::motion { + +/// Double-S (7-phase) trajectory with bounded velocity, acceleration, and jerk. +/// +/// Generates smooth S-curve trajectories consisting of 7 phases: +/// jerk ramp-up, constant acceleration, jerk ramp-down, constant velocity, +/// jerk phase, constant deceleration, jerk ramp-down. +class INTERPOLATECPP_API DoubleSTrajectory { + public: + /// Construct a double-S trajectory. + /// + /// @param state Initial and final state parameters + /// @param bounds Velocity, acceleration, and jerk bounds + DoubleSTrajectory(const StateParams& state, const TrajectoryBounds& bounds); + + /// Evaluate trajectory at time t (returns position, velocity, acceleration, jerk). + [[nodiscard]] FullTrajectoryResult evaluate(double t) const; + + /// Get total trajectory duration. + [[nodiscard]] double duration() const noexcept { return T_; } + + /// Get phase durations. + [[nodiscard]] std::map phase_durations() const; + + private: + static constexpr double kEpsilon = 1e-6; + static constexpr double kMinGamma = 0.01; + static constexpr int kMaxIterations = 50; + + StateParams state_; + TrajectoryBounds bounds_; + + // Planned trajectory parameters + double T_; // Total duration + double Ta_; // Acceleration phase duration + double Tv_; // Constant velocity phase duration + double Td_; // Deceleration phase duration + double Tj_1_; // Jerk time (acceleration) + double Tj_2_; // Jerk time (deceleration) + double a_lim_a_; // Limiting acceleration (accel phase) + double a_lim_d_; // Limiting acceleration (decel phase) + double v_lim_; // Limiting velocity + double sigma_; // Direction (+1 or -1) + + void plan_trajectory(); + [[nodiscard]] FullTrajectoryResult evaluate_7phase(double t) const; +}; + +} // namespace interpolatecpp::motion diff --git a/cpp/include/interpolatecpp/motion/motion_types.hpp b/cpp/include/interpolatecpp/motion/motion_types.hpp new file mode 100644 index 0000000..bc0b215 --- /dev/null +++ b/cpp/include/interpolatecpp/motion/motion_types.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include + +#include + +namespace interpolatecpp::motion { + +/// Result of trajectory evaluation: position, velocity, acceleration. +struct TrajectoryResult { + double position; + double velocity; + double acceleration; +}; + +/// Result of full trajectory evaluation with jerk. +struct FullTrajectoryResult { + double position; + double velocity; + double acceleration; + double jerk; +}; + +/// Boundary condition for polynomial trajectory. +struct BoundaryCondition { + double position = 0.0; + double velocity = 0.0; + double acceleration = 0.0; + double jerk = 0.0; +}; + +/// Time interval for trajectory segments. +struct TimeInterval { + double start = 0.0; + double end = 0.0; + + [[nodiscard]] double duration() const noexcept { return end - start; } +}; + +/// State parameters for trajectory planning (immutable). +struct INTERPOLATECPP_API StateParams { + double q_0; + double q_1; + double v_0; + double v_1; +}; + +/// Trajectory bounds (positive values enforced). +struct INTERPOLATECPP_API TrajectoryBounds { + double v_bound; + double a_bound; + double j_bound; + + TrajectoryBounds(double v, double a, double j) + : v_bound(std::abs(v)), a_bound(std::abs(a)), j_bound(std::abs(j)) { + if (v_bound <= 0.0 || a_bound <= 0.0 || j_bound <= 0.0) { + throw std::invalid_argument("Bounds must be positive values"); + } + } +}; + +} // namespace interpolatecpp::motion diff --git a/cpp/include/interpolatecpp/motion/parabolic_blend_trajectory.hpp b/cpp/include/interpolatecpp/motion/parabolic_blend_trajectory.hpp new file mode 100644 index 0000000..33f7941 --- /dev/null +++ b/cpp/include/interpolatecpp/motion/parabolic_blend_trajectory.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +#include +#include + +namespace interpolatecpp::motion { + +/// Linear segments with parabolic blends trajectory. +/// +/// Creates a trajectory with constant-velocity segments connected by +/// parabolic acceleration blends at each waypoint. +class INTERPOLATECPP_API ParabolicBlendTrajectory { + public: + /// Construct from waypoints with blend durations. + /// + /// @param q Position waypoints + /// @param t Nominal times at each waypoint + /// @param dt_blend Blend duration at each waypoint + ParabolicBlendTrajectory(const std::vector& q, const std::vector& t, + const std::vector& dt_blend); + + /// Evaluate trajectory at time t. + [[nodiscard]] TrajectoryResult evaluate(double t) const; + + /// Get total trajectory duration. + [[nodiscard]] double duration() const; + + /// Get number of waypoints. + [[nodiscard]] int n_waypoints() const noexcept { return n_; } + + private: + int n_; // Number of waypoints + Eigen::VectorXd q_; + Eigen::VectorXd t_; + Eigen::VectorXd dt_blend_; + + // Region data (2*N-1 regions: blend + CV pairs) + Eigen::VectorXd reg_t0_; + Eigen::VectorXd reg_t1_; + Eigen::VectorXd reg_q0_; + Eigen::VectorXd reg_v0_; + Eigen::VectorXd reg_a_; + int n_regions_; + + void build_regions(); + [[nodiscard]] int find_region(double t_abs) const; +}; + +} // namespace interpolatecpp::motion diff --git a/cpp/include/interpolatecpp/motion/polynomial_trajectory.hpp b/cpp/include/interpolatecpp/motion/polynomial_trajectory.hpp new file mode 100644 index 0000000..6ca1139 --- /dev/null +++ b/cpp/include/interpolatecpp/motion/polynomial_trajectory.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include +#include + +#include +#include + +namespace interpolatecpp::motion { + +/// Polynomial trajectory generation supporting orders 3, 5, and 7. +/// +/// Generates smooth trajectories using polynomial interpolation between +/// boundary conditions, with analytical derivative computation. +class INTERPOLATECPP_API PolynomialTrajectory { + public: + static constexpr int ORDER_3 = 3; + static constexpr int ORDER_5 = 5; + static constexpr int ORDER_7 = 7; + + /// Generate an order-3 (cubic) polynomial trajectory. + /// Satisfies position and velocity boundary conditions. + PolynomialTrajectory(const BoundaryCondition& bc_start, const BoundaryCondition& bc_end, + const TimeInterval& interval, int order); + + /// Evaluate trajectory at time t. + [[nodiscard]] FullTrajectoryResult evaluate(double t) const; + + // Accessors + [[nodiscard]] int order() const noexcept { return order_; } + [[nodiscard]] double t_start() const noexcept { return t_start_; } + [[nodiscard]] double t_end() const noexcept { return t_end_; } + [[nodiscard]] double duration() const noexcept { return t_end_ - t_start_; } + [[nodiscard]] const Eigen::VectorXd& coefficients() const noexcept { return coeffs_; } + + /// Compute heuristic velocities for intermediate waypoints. + [[nodiscard]] static std::vector heuristic_velocities( + const std::vector& points, const std::vector& times); + + /// Create multipoint trajectory from waypoints. + [[nodiscard]] static std::vector multipoint_trajectory( + const std::vector& points, const std::vector& times, + int order = ORDER_3, double v0 = 0.0, double vn = 0.0); + + /// Evaluate a multipoint trajectory at time t. + [[nodiscard]] static FullTrajectoryResult evaluate_multipoint( + const std::vector& segments, double t); + + private: + int order_; + double t_start_; + double t_end_; + Eigen::VectorXd coeffs_; + + void compute_order3(const BoundaryCondition& start, const BoundaryCondition& end, + double h); + void compute_order5(const BoundaryCondition& start, const BoundaryCondition& end, + double h); + void compute_order7(const BoundaryCondition& start, const BoundaryCondition& end, + double h); +}; + +} // namespace interpolatecpp::motion diff --git a/cpp/include/interpolatecpp/motion/trapezoidal_trajectory.hpp b/cpp/include/interpolatecpp/motion/trapezoidal_trajectory.hpp new file mode 100644 index 0000000..306f08d --- /dev/null +++ b/cpp/include/interpolatecpp/motion/trapezoidal_trajectory.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +#include +#include + +namespace interpolatecpp::motion { + +/// Trapezoidal velocity profile trajectory (3-phase: accel, cruise, decel). +class INTERPOLATECPP_API TrapezoidalTrajectory { + public: + /// Construct with velocity/acceleration bounds (computes duration). + TrapezoidalTrajectory(double q0, double q1, double amax, double vmax, + double v0 = 0.0, double v1 = 0.0, double t0 = 0.0); + + /// Tag type to disambiguate duration-based constructor. + struct DurationBased {}; + + /// Construct with fixed duration (computes required acceleration). + TrapezoidalTrajectory(DurationBased, double q0, double q1, double amax, + double v0, double v1, double t0, double duration); + + /// Evaluate trajectory at time t. + [[nodiscard]] TrajectoryResult evaluate(double t) const; + + /// Get total trajectory duration. + [[nodiscard]] double duration() const noexcept { return duration_; } + + /// Get start time. + [[nodiscard]] double t_start() const noexcept { return t0_; } + [[nodiscard]] double t_end() const noexcept { return t0_ + duration_; } + + /// Compute heuristic velocities for waypoints. + [[nodiscard]] static std::vector heuristic_velocities( + const std::vector& points, const std::vector& times, + double vmax); + + /// Interpolate through waypoints using trapezoidal profiles. + [[nodiscard]] static std::vector interpolate_waypoints( + const std::vector& points, double amax, double vmax, + double v0 = 0.0, double vn = 0.0, + const std::vector& times = {}, + const std::vector& velocities = {}); + + /// Evaluate a multipoint trajectory at time t. + [[nodiscard]] static TrajectoryResult evaluate_multipoint( + const std::vector& segments, double t); + + private: + static constexpr double kEpsilon = 1e-10; + + double q0_, q1_; + double v0_, v1_; + double t0_; + double duration_; + double ta_; // Acceleration phase duration + double td_; // Deceleration phase duration + double vv_; // Cruise velocity + int sign_; // Direction + + void plan_velocity_based(double amax, double vmax); + void plan_duration_based(double amax, double total_duration); +}; + +} // namespace interpolatecpp::motion diff --git a/cpp/include/interpolatecpp/path/circular_path.hpp b/cpp/include/interpolatecpp/path/circular_path.hpp new file mode 100644 index 0000000..8040b6b --- /dev/null +++ b/cpp/include/interpolatecpp/path/circular_path.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include + +#include + +namespace interpolatecpp::path { + +/// Arc-length parameterized circular path in 3D space. +class INTERPOLATECPP_API CircularPath { + public: + /// Construct from axis vector, point on axis, and point on circle. + CircularPath(const Eigen::Vector3d& axis, const Eigen::Vector3d& axis_point, + const Eigen::Vector3d& circle_point); + + [[nodiscard]] Eigen::Vector3d position(double s) const; + [[nodiscard]] Eigen::MatrixXd position(const Eigen::VectorXd& s) const; + [[nodiscard]] Eigen::Vector3d velocity(double s) const; + [[nodiscard]] Eigen::Vector3d acceleration(double s) const; + [[nodiscard]] double radius() const noexcept { return radius_; } + [[nodiscard]] const Eigen::Vector3d& center() const noexcept { return center_; } + + private: + Eigen::Vector3d axis_, center_; + Eigen::Matrix3d R_; // Local-to-global rotation + double radius_; +}; + +} // namespace interpolatecpp::path diff --git a/cpp/include/interpolatecpp/path/frenet_frame.hpp b/cpp/include/interpolatecpp/path/frenet_frame.hpp new file mode 100644 index 0000000..4023168 --- /dev/null +++ b/cpp/include/interpolatecpp/path/frenet_frame.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include + +namespace interpolatecpp::path { + +/// Frenet-Serret frame result. +struct FrenetFrame { + Eigen::Vector3d tangent; + Eigen::Vector3d normal; + Eigen::Vector3d binormal; + double curvature; + double torsion; +}; + +/// Compute Frenet-Serret frames along a parametric curve. +/// +/// @param curve Function returning (position, velocity, acceleration) at parameter s +/// @param s_values Parameter values at which to compute frames +/// @return Vector of Frenet frames +[[nodiscard]] INTERPOLATECPP_API std::vector compute_frenet_frames( + const std::function( + double)>& curve, + const Eigen::VectorXd& s_values); + +/// Circular trajectory returning (position, velocity, acceleration). +/// +/// @param u Parameter value +/// @param r Radius (default 2.0) +/// @return Tuple of (position, dp/du, d2p/du2) +[[nodiscard]] INTERPOLATECPP_API +std::tuple +circular_trajectory_with_derivatives(double u, double r = 2.0); + +/// Helicoidal trajectory returning (position, velocity, acceleration). +/// +/// @param u Parameter value +/// @param r Radius (default 2.0) +/// @param d Pitch parameter (default 0.5) +/// @return Tuple of (position, dp/du, d2p/du2) +[[nodiscard]] INTERPOLATECPP_API +std::tuple +helicoidal_trajectory_with_derivatives(double u, double r = 2.0, double d = 0.5); + +} // namespace interpolatecpp::path diff --git a/cpp/include/interpolatecpp/path/linear_path.hpp b/cpp/include/interpolatecpp/path/linear_path.hpp new file mode 100644 index 0000000..d3d2305 --- /dev/null +++ b/cpp/include/interpolatecpp/path/linear_path.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include + +namespace interpolatecpp::path { + +/// Arc-length parameterized linear path in 3D space. +class INTERPOLATECPP_API LinearPath { + public: + LinearPath(const Eigen::Vector3d& pi, const Eigen::Vector3d& pf); + + [[nodiscard]] Eigen::Vector3d position(double s) const; + [[nodiscard]] Eigen::MatrixXd position(const Eigen::VectorXd& s) const; + [[nodiscard]] Eigen::Vector3d velocity(double s) const; + [[nodiscard]] Eigen::Vector3d acceleration(double s) const; + [[nodiscard]] double length() const noexcept { return length_; } + + private: + Eigen::Vector3d pi_, pf_, tangent_; + double length_; +}; + +} // namespace interpolatecpp::path diff --git a/cpp/include/interpolatecpp/path/linear_traj.hpp b/cpp/include/interpolatecpp/path/linear_traj.hpp new file mode 100644 index 0000000..ec8cce4 --- /dev/null +++ b/cpp/include/interpolatecpp/path/linear_traj.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include + +namespace interpolatecpp::path { + +/// Result of linear interpolation. +struct LinearTrajResult { + Eigen::MatrixXd positions; + Eigen::MatrixXd velocities; + Eigen::MatrixXd accelerations; +}; + +/// Simple linear interpolation between two points or scalars. +[[nodiscard]] INTERPOLATECPP_API LinearTrajResult linear_traj( + const Eigen::VectorXd& p0, const Eigen::VectorXd& p1, double t0, double t1, + int num_points = 100); + +} // namespace interpolatecpp::path diff --git a/cpp/include/interpolatecpp/quat/log_quaternion_interpolation.hpp b/cpp/include/interpolatecpp/quat/log_quaternion_interpolation.hpp new file mode 100644 index 0000000..c041a6d --- /dev/null +++ b/cpp/include/interpolatecpp/quat/log_quaternion_interpolation.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace interpolatecpp::quat { + +/// Logarithmic quaternion interpolation (Parker Algorithm 1). +/// +/// Transforms quaternions to axis-angle space, interpolates with B-splines, +/// and maps back. Handles double-cover and axis continuity. +class INTERPOLATECPP_API LogQuaternionInterpolation { + public: + LogQuaternionInterpolation( + const std::vector& time_points, + const std::vector& quaternions, int degree = 3, + const std::optional& initial_velocity = std::nullopt, + const std::optional& final_velocity = std::nullopt); + + [[nodiscard]] Quaternion evaluate(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_velocity(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_acceleration(double t) const; + + [[nodiscard]] double t_min() const noexcept { return times_.front(); } + [[nodiscard]] double t_max() const noexcept { return times_.back(); } + + private: + std::vector times_; + std::vector quaternions_; + std::unique_ptr spline_; + + static constexpr double kEpsilon = 1e-10; + + [[nodiscard]] Eigen::MatrixXd recover_continuous_axis_angle() const; +}; + +} // namespace interpolatecpp::quat diff --git a/cpp/include/interpolatecpp/quat/modified_log_quaternion_interpolation.hpp b/cpp/include/interpolatecpp/quat/modified_log_quaternion_interpolation.hpp new file mode 100644 index 0000000..fac7548 --- /dev/null +++ b/cpp/include/interpolatecpp/quat/modified_log_quaternion_interpolation.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace interpolatecpp::quat { + +/// Modified Logarithmic Quaternion Interpolation (mLQI). +/// +/// Interpolates quaternions as (theta, X, Y, Z) where X^2+Y^2+Z^2=1, +/// using separate B-spline interpolators for angle and axis components. +/// Based on Parker et al. (2023). +class INTERPOLATECPP_API ModifiedLogQuaternionInterpolation { + public: + /// Construct a modified log-quaternion interpolator. + /// + /// @param time_points Strictly increasing time values + /// @param quaternions Unit quaternions at each time point + /// @param degree B-spline degree (3, 4, or 5) + /// @param normalize_axis Whether to normalize (X,Y,Z) after interpolation + /// @param initial_velocity Initial velocity constraint (4D: [theta_dot, X_dot, Y_dot, Z_dot]) + /// @param final_velocity Final velocity constraint (4D) + ModifiedLogQuaternionInterpolation( + const std::vector& time_points, + const std::vector& quaternions, int degree = 3, + bool normalize_axis = true, + const std::optional& initial_velocity = std::nullopt, + const std::optional& final_velocity = std::nullopt); + + [[nodiscard]] Quaternion evaluate(double t) const; + [[nodiscard]] Eigen::Vector4d evaluate_velocity(double t) const; + [[nodiscard]] Eigen::Vector4d evaluate_acceleration(double t) const; + + [[nodiscard]] double t_min() const noexcept { return times_.front(); } + [[nodiscard]] double t_max() const noexcept { return times_.back(); } + [[nodiscard]] bool normalize_axis() const noexcept { return normalize_axis_; } + + private: + std::vector times_; + std::vector quaternions_; + bool normalize_axis_; + + std::unique_ptr theta_spline_; + std::unique_ptr xyz_spline_; + + static constexpr double kEpsilon = 1e-10; + + void ensure_quaternion_continuity(); + [[nodiscard]] std::pair + transform_to_theta_xyz() const; +}; + +} // namespace interpolatecpp::quat diff --git a/cpp/include/interpolatecpp/quat/quaternion.hpp b/cpp/include/interpolatecpp/quat/quaternion.hpp new file mode 100644 index 0000000..17cb1b3 --- /dev/null +++ b/cpp/include/interpolatecpp/quat/quaternion.hpp @@ -0,0 +1,99 @@ +#pragma once + +#include +#include + +#include + +namespace interpolatecpp::quat { + +/// Quaternion class with exp/log/power/slerp/squad operations. +/// +/// Stores as [w, x, y, z] (scalar-first convention). +/// Wraps Eigen::Quaterniond but adds mathematical operations needed +/// for trajectory interpolation that Eigen doesn't provide. +class INTERPOLATECPP_API Quaternion { + public: + /// Construct from components (scalar-first). + explicit Quaternion(double w = 1.0, double x = 0.0, double y = 0.0, double z = 0.0); + + /// Construct from Eigen quaternion. + explicit Quaternion(const Eigen::Quaterniond& eq); + + // Factory methods + [[nodiscard]] static Quaternion identity(); + [[nodiscard]] static Quaternion from_angle_axis(double angle, + const Eigen::Vector3d& axis); + [[nodiscard]] static Quaternion from_euler_angles(double roll, double pitch, + double yaw); + + // Component access + [[nodiscard]] double w() const noexcept { return q_.w(); } + [[nodiscard]] double x() const noexcept { return q_.x(); } + [[nodiscard]] double y() const noexcept { return q_.y(); } + [[nodiscard]] double z() const noexcept { return q_.z(); } + [[nodiscard]] Eigen::Vector3d vec() const noexcept { return q_.vec(); } + [[nodiscard]] const Eigen::Quaterniond& eigen() const noexcept { return q_; } + + // Arithmetic (immutable - returns new Quaternion) + [[nodiscard]] Quaternion operator*(const Quaternion& other) const; + [[nodiscard]] Quaternion operator*(double scalar) const; + [[nodiscard]] Quaternion operator+(const Quaternion& other) const; + [[nodiscard]] Quaternion operator-(const Quaternion& other) const; + [[nodiscard]] Quaternion operator-() const; + + // Core operations + [[nodiscard]] Quaternion conjugate() const; + [[nodiscard]] Quaternion inverse() const; + [[nodiscard]] Quaternion unit() const; + [[nodiscard]] double norm() const noexcept; + [[nodiscard]] double norm_squared() const noexcept; + [[nodiscard]] double dot_product(const Quaternion& other) const; + + // Exponential/logarithmic maps + [[nodiscard]] static Quaternion exp(const Quaternion& q); + [[nodiscard]] static Quaternion log(const Quaternion& q); + [[nodiscard]] static Quaternion power(const Quaternion& q, double t); + + // Interpolation + [[nodiscard]] static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, + double t); + [[nodiscard]] static Quaternion slerp_prime(const Quaternion& q0, + const Quaternion& q1, double t); + [[nodiscard]] static Quaternion squad(const Quaternion& p, const Quaternion& a, + const Quaternion& b, const Quaternion& q, + double t); + [[nodiscard]] static Quaternion compute_intermediate_quaternion( + const Quaternion& q_prev, const Quaternion& q_curr, const Quaternion& q_next); + + // Conversions + [[nodiscard]] Eigen::Matrix3d to_rotation_matrix() const; + [[nodiscard]] Eigen::Matrix4d to_transformation_matrix() const; + [[nodiscard]] std::pair to_axis_angle() const; + [[nodiscard]] std::tuple to_euler_angles() const; + + // Factory: construct from rotation matrix (3x3) + [[nodiscard]] static Quaternion from_rotation_matrix( + const Eigen::Matrix3d& rotation_matrix); + + // Dynamics + /// E-matrix for quaternion kinematics. sign=0 โ†’ base frame, sign=1 โ†’ body frame. + [[nodiscard]] Eigen::Matrix3d E(int sign) const; + + /// Quaternion time derivative qdot = 0.5 * E(sign) * omega. + [[nodiscard]] Quaternion dot(const Eigen::Vector3d& omega, int sign) const; + + /// Extract angular velocity from q and qdot. + [[nodiscard]] static Eigen::Vector3d Omega(const Quaternion& q, + const Quaternion& q_dot); + + // Conversion to Eigen::Quaterniond for concept conformance + operator Eigen::Quaterniond() const noexcept { return q_; } + + private: + Eigen::Quaterniond q_; + + static constexpr double kEpsilon = 1e-7; +}; + +} // namespace interpolatecpp::quat diff --git a/cpp/include/interpolatecpp/quat/quaternion_spline.hpp b/cpp/include/interpolatecpp/quat/quaternion_spline.hpp new file mode 100644 index 0000000..a1eb256 --- /dev/null +++ b/cpp/include/interpolatecpp/quat/quaternion_spline.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace interpolatecpp::quat { + +/// Quaternion spline interpolation with SLERP/SQUAD/Auto methods. +class INTERPOLATECPP_API QuaternionSpline { + public: + enum class Method { Slerp, Squad, Auto }; + + QuaternionSpline(const std::vector& time_points, + const std::vector& quaternions, + Method method = Method::Auto); + + [[nodiscard]] Quaternion evaluate(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_velocity(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_acceleration(double t) const; + + [[nodiscard]] double t_min() const noexcept { return times_.front(); } + [[nodiscard]] double t_max() const noexcept { return times_.back(); } + + private: + std::vector times_; + std::vector quaternions_; + std::vector intermediates_; + Method method_; + + static constexpr double kDt = 1e-6; + + void compute_intermediates(); + [[nodiscard]] int find_segment(double t) const; +}; + +} // namespace interpolatecpp::quat diff --git a/cpp/include/interpolatecpp/quat/squad_c2.hpp b/cpp/include/interpolatecpp/quat/squad_c2.hpp new file mode 100644 index 0000000..a7f251f --- /dev/null +++ b/cpp/include/interpolatecpp/quat/squad_c2.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +#include +#include + +namespace interpolatecpp::quat { + +/// Configuration for SquadC2 interpolation. +struct SquadC2Config { + std::vector time_points; + std::vector quaternions; + bool normalize_quaternions = true; + /// Reserved for future use: when true, numerical C2-continuity + /// validation will be performed after construction. + bool validate_continuity = true; +}; + +/// C2-continuous SQUAD quaternion interpolation (Wittmann et al.). +/// +/// Uses quintic polynomial parameterization for zero-clamped boundary +/// conditions, ensuring continuous angular velocity and acceleration. +class INTERPOLATECPP_API SquadC2 { + public: + SquadC2(const std::vector& time_points, + const std::vector& quaternions, + bool normalize_quaternions = true, + bool validate_continuity = true); + + /// Construct from config. + explicit SquadC2(const SquadC2Config& config); + + [[nodiscard]] Quaternion evaluate(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_velocity(double t) const; + [[nodiscard]] Eigen::Vector3d evaluate_acceleration(double t) const; + + [[nodiscard]] double t_min() const noexcept { return times_.front(); } + [[nodiscard]] double t_max() const noexcept { return times_.back(); } + [[nodiscard]] bool validate_continuity() const noexcept { return validate_continuity_; } + + private: + std::vector times_; + std::vector quaternions_; + bool validate_continuity_; + + // Extended sequence (with virtual waypoints) + std::vector ext_times_; + std::vector ext_quats_; + std::vector ext_intermediates_; + + static constexpr double kDt = 1e-6; + + void build_extended_sequence(); + void compute_intermediates(); + [[nodiscard]] int find_original_segment(double t) const; + [[nodiscard]] static double quintic_u(double t, double t0, double t1); +}; + +} // namespace interpolatecpp::quat diff --git a/cpp/include/interpolatecpp/spline/cubic_smoothing_spline.hpp b/cpp/include/interpolatecpp/spline/cubic_smoothing_spline.hpp new file mode 100644 index 0000000..c976f5d --- /dev/null +++ b/cpp/include/interpolatecpp/spline/cubic_smoothing_spline.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include + +#include + +namespace interpolatecpp::spline { + +/// Cubic smoothing spline with accuracy-smoothness trade-off. +/// +/// Minimizes a weighted sum of waypoint error and acceleration magnitude. +/// mu=1 gives exact interpolation; mu->0 gives maximum smoothing. +class INTERPOLATECPP_API CubicSmoothingSpline { + public: + static constexpr int kMinPointsRequired = 2; + static constexpr double kHighConditionThreshold = 1e12; + static constexpr double kRegularizationFactor = 1e-8; + + /// @param t_points Time points (strictly increasing) + /// @param q_points Position points + /// @param mu Smoothing parameter in (0, 1]. Default 0.5 + /// @param weights Per-point weights (nullopt = uniform 1.0) + /// @param v0 Initial velocity (default 0) + /// @param vn Final velocity (default 0) + /// @param debug Print debug info (default false) + CubicSmoothingSpline(std::span t_points, std::span q_points, + double mu = 0.5, + std::optional> weights = std::nullopt, + double v0 = 0.0, double vn = 0.0, bool debug = false); + + // Evaluation (satisfies ScalarTrajectory concept) + [[nodiscard]] double evaluate(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_velocity(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_velocity(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_acceleration(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_acceleration(const Eigen::VectorXd& t) const; + + // Accessors + [[nodiscard]] const Eigen::VectorXd& t_points() const noexcept { return t_; } + [[nodiscard]] const Eigen::VectorXd& q_points() const noexcept { return q_; } + [[nodiscard]] const Eigen::VectorXd& s_points() const noexcept { return s_; } + [[nodiscard]] const Eigen::VectorXd& omega() const noexcept { return omega_; } + [[nodiscard]] const Eigen::MatrixXd& coefficients() const noexcept { return coeffs_; } + [[nodiscard]] double mu() const noexcept { return mu_; } + [[nodiscard]] double lambda() const noexcept { return lambd_; } + [[nodiscard]] int n_points() const noexcept { return n_; } + + private: + Eigen::VectorXd t_; + Eigen::VectorXd q_; + Eigen::VectorXd s_; // approximated positions + Eigen::VectorXd omega_; // accelerations + Eigen::VectorXd w_; // weights + Eigen::VectorXd w_inv_; // inverse weights + Eigen::MatrixXd coeffs_; // (n-1) x 4 + Eigen::MatrixXd a_matrix_; // tridiagonal A + Eigen::MatrixXd c_matrix_; // C matrix + + double mu_; + double lambd_; + double v0_; + double vn_; + bool debug_; + int n_; // number of points + Eigen::VectorXd time_intervals_; + + void construct_matrices(); + void solve_system(); + void compute_positions(); + void compute_coefficients(); + + struct SegmentInfo { + int index; + double tau; + }; + [[nodiscard]] SegmentInfo find_segment(double t) const; +}; + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/spline/cubic_spline.hpp b/cpp/include/interpolatecpp/spline/cubic_spline.hpp new file mode 100644 index 0000000..bfe8c32 --- /dev/null +++ b/cpp/include/interpolatecpp/spline/cubic_spline.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include +#include + +#include + +namespace interpolatecpp::spline { + +/// C2-continuous cubic spline trajectory planning. +/// +/// Generates a smooth trajectory passing through specified waypoints +/// with continuous position, velocity, and acceleration profiles. +/// Polynomial per segment: q(tau) = a0 + a1*tau + a2*tau^2 + a3*tau^3 +class INTERPOLATECPP_API CubicSpline { + public: + /// Construct a cubic spline through the given waypoints. + /// + /// @param t_points Time points (must be strictly increasing) + /// @param q_points Position points (same length as t_points) + /// @param v0 Initial velocity (default 0) + /// @param vn Final velocity (default 0) + /// @param debug Print debug info (default false) + /// @throws std::invalid_argument if lengths mismatch or times not increasing + CubicSpline(std::span t_points, std::span q_points, + double v0 = 0.0, double vn = 0.0, bool debug = false); + + virtual ~CubicSpline() = default; + + // Evaluation (satisfies ScalarTrajectory concept) + [[nodiscard]] double evaluate(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate(const Eigen::VectorXd& t) const; + + [[nodiscard]] double evaluate_velocity(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_velocity(const Eigen::VectorXd& t) const; + + [[nodiscard]] double evaluate_acceleration(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_acceleration(const Eigen::VectorXd& t) const; + + // Accessors + [[nodiscard]] const Eigen::VectorXd& t_points() const noexcept { return t_points_; } + [[nodiscard]] const Eigen::VectorXd& q_points() const noexcept { return q_points_; } + [[nodiscard]] const Eigen::VectorXd& t_intervals() const noexcept { return t_intervals_; } + [[nodiscard]] const Eigen::VectorXd& velocities() const noexcept { return velocities_; } + [[nodiscard]] const Eigen::MatrixXd& coefficients() const noexcept { return coefficients_; } + [[nodiscard]] int n_segments() const noexcept { return n_; } + + protected: + Eigen::VectorXd t_points_; + Eigen::VectorXd q_points_; + double v0_; + double vn_; + bool debug_; + int n_; // number of segments + Eigen::VectorXd t_intervals_; + Eigen::VectorXd velocities_; + Eigen::MatrixXd coefficients_; // (n_ x 4) + + virtual void compute_velocities(); + virtual void compute_coefficients(); + + struct SegmentInfo { + int index; + double tau; + }; + + /// Find segment containing time t and local parameter tau. + [[nodiscard]] SegmentInfo find_segment(double t) const; +}; + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/spline/cubic_spline_with_acc1.hpp b/cpp/include/interpolatecpp/spline/cubic_spline_with_acc1.hpp new file mode 100644 index 0000000..a7fffc6 --- /dev/null +++ b/cpp/include/interpolatecpp/spline/cubic_spline_with_acc1.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include + +#include + +namespace interpolatecpp::spline { + +/// Cubic spline with velocity AND acceleration constraints at endpoints. +/// +/// Adds two extra points at midpoints of first/last segments to satisfy +/// acceleration boundary conditions (section 4.4.4 of the reference). +class INTERPOLATECPP_API CubicSplineWithAcceleration1 { + public: + /// @param t_points Original time points (strictly increasing) + /// @param q_points Original position points + /// @param v0 Initial velocity (default 0) + /// @param vn Final velocity (default 0) + /// @param a0 Initial acceleration (default 0) + /// @param an Final acceleration (default 0) + /// @param debug Print debug info (default false) + CubicSplineWithAcceleration1(std::span t_points, + std::span q_points, double v0 = 0.0, + double vn = 0.0, double a0 = 0.0, double an = 0.0, + bool debug = false); + + // Evaluation (satisfies ScalarTrajectory concept) + [[nodiscard]] double evaluate(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_velocity(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_velocity(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_acceleration(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_acceleration(const Eigen::VectorXd& t) const; + + // Accessors + [[nodiscard]] const Eigen::VectorXd& t_points() const noexcept { return t_; } + [[nodiscard]] const Eigen::VectorXd& q_points() const noexcept { return q_; } + [[nodiscard]] const Eigen::VectorXd& t_orig() const noexcept { return t_orig_; } + [[nodiscard]] const Eigen::VectorXd& q_orig() const noexcept { return q_orig_; } + [[nodiscard]] const Eigen::VectorXd& omega() const noexcept { return omega_; } + [[nodiscard]] const Eigen::MatrixXd& coefficients() const noexcept { return coeffs_; } + [[nodiscard]] const std::vector& original_indices() const noexcept { return original_indices_; } + [[nodiscard]] int n_points() const noexcept { return n_; } + [[nodiscard]] int n_orig() const noexcept { return n_orig_; } + + private: + Eigen::VectorXd t_orig_; + Eigen::VectorXd q_orig_; + Eigen::VectorXd t_; // expanded with extra points + Eigen::VectorXd q_; // expanded with extra points + Eigen::VectorXd T_; // time intervals + Eigen::VectorXd omega_; // accelerations + Eigen::MatrixXd coeffs_; // (n-1) x 4 + + double v0_, vn_, a0_, an_; + bool debug_; + int n_orig_; + int n_; + + std::vector original_indices_; + + void add_extra_points(); + void solve_accelerations(); + void compute_coefficients(); + void compute_original_indices(); + + struct SegmentInfo { + int index; + double tau; + }; + [[nodiscard]] SegmentInfo find_segment(double t) const; +}; + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/spline/cubic_spline_with_acc2.hpp b/cpp/include/interpolatecpp/spline/cubic_spline_with_acc2.hpp new file mode 100644 index 0000000..098aabc --- /dev/null +++ b/cpp/include/interpolatecpp/spline/cubic_spline_with_acc2.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace interpolatecpp::spline { + +/// Cubic spline with quintic polynomial segments for acceleration constraints. +/// +/// Extends CubicSpline: replaces first/last segments with 5th-degree polynomials +/// when acceleration constraints are provided (section 4.4.4 of the reference). +/// +/// NOTE: Evaluate methods intentionally hide (not override) the base class +/// non-virtual methods. Do not call through a CubicSpline* pointer โ€” use the +/// concrete CubicSplineWithAcceleration2 type directly. +class INTERPOLATECPP_API CubicSplineWithAcceleration2 : public CubicSpline { + public: + using QuinticCoeffs = Eigen::Vector; + + CubicSplineWithAcceleration2(std::span t_points, + std::span q_points, + const SplineParameters& params = SplineParameters{}); + + // Override evaluation to dispatch to quintic segments + [[nodiscard]] double evaluate(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_velocity(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_velocity(const Eigen::VectorXd& t) const; + [[nodiscard]] double evaluate_acceleration(double t) const; + [[nodiscard]] Eigen::VectorXd evaluate_acceleration(const Eigen::VectorXd& t) const; + + // Accessors + [[nodiscard]] std::optional a0() const noexcept { return a0_; } + [[nodiscard]] std::optional an() const noexcept { return an_; } + [[nodiscard]] bool has_quintic_first() const noexcept { return quintic_first_.has_value(); } + [[nodiscard]] bool has_quintic_last() const noexcept { return quintic_last_.has_value(); } + + private: + std::optional a0_; + std::optional an_; + std::optional quintic_first_; + std::optional quintic_last_; + + void replace_first_segment_with_quintic(); + void replace_last_segment_with_quintic(); +}; + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/spline/smoothing_search.hpp b/cpp/include/interpolatecpp/spline/smoothing_search.hpp new file mode 100644 index 0000000..d24018f --- /dev/null +++ b/cpp/include/interpolatecpp/spline/smoothing_search.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#include +#include +#include + +namespace interpolatecpp::spline { + +/// Result of smoothing spline tolerance search. +struct SmoothingSearchResult { + CubicSmoothingSpline spline; + double mu; + double max_error; + int iterations; +}; + +/// Find optimal smoothing parameter mu via binary search on tolerance. +/// +/// Searches for the smallest mu (most smoothing) that keeps +/// max|q - s| below the given tolerance. +/// +/// @param t_points Time points +/// @param q_points Position points +/// @param tolerance Maximum allowed approximation error +/// @param config Search configuration +/// @return SmoothingSearchResult with optimal spline, mu, error, iterations +INTERPOLATECPP_API SmoothingSearchResult +smoothing_spline_with_tolerance(std::span t_points, + std::span q_points, double tolerance, + const SplineConfig& config); + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/spline/spline_parameters.hpp b/cpp/include/interpolatecpp/spline/spline_parameters.hpp new file mode 100644 index 0000000..d4ab97b --- /dev/null +++ b/cpp/include/interpolatecpp/spline/spline_parameters.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +namespace interpolatecpp::spline { + +/// Parameters for CubicSplineWithAcceleration2 construction. +/// Mirrors Python's SplineParameters dataclass. +struct SplineParameters { + double v0 = 0.0; + double vn = 0.0; + std::optional a0 = std::nullopt; + std::optional an = std::nullopt; + bool debug = false; +}; + +/// Configuration for smoothing spline tolerance search. +/// Mirrors Python's SplineConfig dataclass. +struct SplineConfig { + std::optional weights = std::nullopt; + double v0 = 0.0; + double vn = 0.0; + int max_iterations = 50; + bool debug = false; +}; + +} // namespace interpolatecpp::spline diff --git a/cpp/include/interpolatecpp/tridiagonal.hpp b/cpp/include/interpolatecpp/tridiagonal.hpp new file mode 100644 index 0000000..c09e898 --- /dev/null +++ b/cpp/include/interpolatecpp/tridiagonal.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +namespace interpolatecpp { + +/// Solve a tridiagonal system Ax = d using the Thomas algorithm. +/// +/// Takes vectors by value (creates working copies, matching Python's np.array() copy). +/// O(n) complexity instead of O(n^3) for general matrix solvers. +/// +/// @param lower_diagonal Lower diagonal (first element unused) +/// @param main_diagonal Main diagonal +/// @param upper_diagonal Upper diagonal (last element unused) +/// @param right_hand_side Right-hand side vector +/// @return Solution vector x +/// @throws std::invalid_argument if a pivot is zero +inline Eigen::VectorXd solve_tridiagonal(Eigen::VectorXd lower_diagonal, + Eigen::VectorXd main_diagonal, + Eigen::VectorXd upper_diagonal, + Eigen::VectorXd right_hand_side) { + const auto n = right_hand_side.size(); + + if (main_diagonal(0) == 0.0) { + throw std::invalid_argument( + "Pivot cannot be zero. The system cannot be solved with this method."); + } + + // Forward elimination + for (Eigen::Index k = 1; k < n; ++k) { + double m = lower_diagonal(k) / main_diagonal(k - 1); + main_diagonal(k) -= m * upper_diagonal(k - 1); + right_hand_side(k) -= m * right_hand_side(k - 1); + } + + // Back substitution + Eigen::VectorXd x(n); + x(n - 1) = right_hand_side(n - 1) / main_diagonal(n - 1); + for (Eigen::Index k = n - 2; k >= 0; --k) { + x(k) = (right_hand_side(k) - upper_diagonal(k) * x(k + 1)) / main_diagonal(k); + } + + return x; +} + +} // namespace interpolatecpp diff --git a/cpp/src/approximation_bspline.cpp b/cpp/src/approximation_bspline.cpp new file mode 100644 index 0000000..d8303c1 --- /dev/null +++ b/cpp/src/approximation_bspline.cpp @@ -0,0 +1,208 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::bspline { + +Eigen::VectorXd ApproximationBSpline::compute_parameters(const Eigen::MatrixXd& points, + Parameterization method) { + const int n = static_cast(points.rows()) - 1; + Eigen::VectorXd u_bar = Eigen::VectorXd::Zero(n + 1); + u_bar[0] = 0.0; + u_bar[n] = 1.0; + + switch (method) { + case Parameterization::EquallySpaced: { + for (int k = 1; k < n; ++k) { + u_bar[k] = static_cast(k) / static_cast(n); + } + break; + } + case Parameterization::ChordLength: { + double total = 0.0; + for (int k = 1; k <= n; ++k) { + total += (points.row(k) - points.row(k - 1)).norm(); + } + for (int k = 1; k < n; ++k) { + u_bar[k] = + u_bar[k - 1] + (points.row(k) - points.row(k - 1)).norm() / total; + } + break; + } + case Parameterization::Centripetal: { + constexpr double mu = 0.5; + double total = 0.0; + for (int k = 1; k <= n; ++k) { + total += std::pow((points.row(k) - points.row(k - 1)).norm(), mu); + } + for (int k = 1; k < n; ++k) { + u_bar[k] = u_bar[k - 1] + + std::pow((points.row(k) - points.row(k - 1)).norm(), mu) / total; + } + break; + } + } + + return u_bar; +} + +Eigen::VectorXd ApproximationBSpline::compute_knots(int degree, int num_control_points, + int num_points, + const Eigen::VectorXd& u_bar) { + const int num_knots = num_control_points + degree + 1; + Eigen::VectorXd knots = Eigen::VectorXd::Zero(num_knots); + + // First and last knots with multiplicity p+1 + for (int i = 0; i <= degree; ++i) { + knots[i] = u_bar[0]; + } + for (int i = num_knots - degree - 1; i < num_knots; ++i) { + knots[i] = u_bar[u_bar.size() - 1]; + } + + // Internal knots (Section 8.5.1) + const int n = num_points - 1; + const int m = num_control_points - 1; + const double d = static_cast(n + 1) / static_cast(m - degree + 1); + + for (int j = 1; j <= m - degree; ++j) { + int i = static_cast(j * d); + double alpha = j * d - i; + knots[j + degree] = (1.0 - alpha) * u_bar[i - 1] + alpha * u_bar[i]; + } + + return knots; +} + +Eigen::MatrixXd ApproximationBSpline::approximate_control_points( + const Eigen::MatrixXd& points, int degree, const Eigen::VectorXd& knots, + const Eigen::VectorXd& u_bar, int num_control_points, + const Eigen::VectorXd& weights) const { + const int n = static_cast(points.rows()) - 1; + const int m = num_control_points - 1; + const int dim = static_cast(points.cols()); + + Eigen::MatrixXd cp = Eigen::MatrixXd::Zero(num_control_points, dim); + cp.row(0) = points.row(0); + cp.row(m) = points.row(n); + + if (m <= 1) { + return cp; + } + + // Create temporary BSpline for basis function calculation + Eigen::MatrixXd temp_ctrl = Eigen::MatrixXd::Zero(num_control_points, dim); + BSpline temp_bs(degree, std::span(knots.data(), knots.size()), temp_ctrl); + + // Build B matrix (n-1 x m-1) and R matrix (n-1 x dim) + Eigen::MatrixXd b_matrix = Eigen::MatrixXd::Zero(n - 1, m - 1); + Eigen::MatrixXd r_matrix = Eigen::MatrixXd::Zero(n - 1, dim); + + for (int k = 1; k < n; ++k) { + const double u = u_bar[k]; + const int span = temp_bs.find_knot_span(u); + const Eigen::VectorXd basis_vals = temp_bs.basis_functions(u, span); + + // Collect all basis function values + Eigen::VectorXd all_basis = Eigen::VectorXd::Zero(m + 1); + for (int j = 0; j <= degree; ++j) { + int idx = span - degree + j; + if (idx >= 0 && idx <= m) { + all_basis[idx] = basis_vals[j]; + } + } + + // Fill B matrix with internal control point basis values + for (int j = 1; j < m; ++j) { + b_matrix(k - 1, j - 1) = all_basis[j]; + } + + // R = Q - B_0 * Q_0 - B_m * Q_n + r_matrix.row(k - 1) = + points.row(k) - all_basis[0] * points.row(0) - all_basis[m] * points.row(n); + } + + // Weighted least squares: (B^T W B)^{-1} B^T W R + Eigen::MatrixXd w_matrix = Eigen::MatrixXd::Zero(n - 1, n - 1); + for (int i = 0; i < n - 1; ++i) { + w_matrix(i, i) = weights[i]; + } + + Eigen::MatrixXd btw = b_matrix.transpose() * w_matrix; + Eigen::MatrixXd btwb = btw * b_matrix; + Eigen::MatrixXd btwr = btw * r_matrix; + + // Solve normal equations + Eigen::MatrixXd internal_cp; + Eigen::FullPivLU lu(btwb); + if (lu.isInvertible()) { + internal_cp = lu.solve(btwr); + } else { + // Fallback to pseudo-inverse + internal_cp = btwb.completeOrthogonalDecomposition().solve(btwr); + } + + cp.block(1, 0, m - 1, dim) = internal_cp; + + return cp; +} + +double ApproximationBSpline::calculate_approximation_error() const { + double sum_sq = 0.0; + for (int i = 0; i < static_cast(original_points_.rows()); ++i) { + Eigen::VectorXd spline_pt = evaluate(original_parameters_[i]); + double sq_dist = (original_points_.row(i).transpose() - spline_pt).squaredNorm(); + sum_sq += sq_dist; + } + return sum_sq; +} + +ApproximationBSpline::ApproximationBSpline(const Eigen::MatrixXd& points, + int num_control_points, int degree, + const std::optional& weights, + Parameterization method, bool debug) + : BSpline(DeferInit{}) + , + debug_(debug) { + if (degree < 1) { + throw std::invalid_argument("Degree must be at least 1"); + } + if (num_control_points <= degree) { + throw std::invalid_argument( + "Number of control points must be greater than the degree"); + } + if (static_cast(points.rows()) <= num_control_points) { + throw std::invalid_argument( + "Number of points must be greater than number of control points"); + } + + const int n = static_cast(points.rows()) - 1; + + Eigen::VectorXd w; + if (weights.has_value()) { + w = weights.value(); + } else { + w = Eigen::VectorXd::Ones(n - 1); + } + + Eigen::VectorXd u_bar = compute_parameters(points, method); + Eigen::VectorXd new_knots = + compute_knots(degree, num_control_points, static_cast(points.rows()), u_bar); + Eigen::MatrixXd new_cp = + approximate_control_points(points, degree, new_knots, u_bar, num_control_points, w); + + // Reinitialize base + degree_ = degree; + knots_ = new_knots; + control_points_ = new_cp; + u_min_ = knots_[degree]; + u_max_ = knots_[static_cast(knots_.size()) - degree - 1]; + dimension_ = static_cast(points.cols()); + + original_points_ = points; + original_parameters_ = u_bar; +} + +} // namespace interpolatecpp::bspline diff --git a/cpp/src/bspline.cpp b/cpp/src/bspline.cpp new file mode 100644 index 0000000..94608e7 --- /dev/null +++ b/cpp/src/bspline.cpp @@ -0,0 +1,308 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::bspline { + +BSpline::BSpline(int degree, std::span knots, + const Eigen::MatrixXd& control_points) + : degree_(degree), control_points_(control_points) { + + if (degree < 0) { + throw std::invalid_argument("Degree must be non-negative"); + } + + // Validate knot vector is non-decreasing + for (size_t i = 1; i < knots.size(); ++i) { + if (knots[i] < knots[i - 1]) { + throw std::invalid_argument("Knot vector must be non-decreasing"); + } + } + + const auto n_control = static_cast(control_points.rows()); + const auto n_knots = static_cast(knots.size()); + + if (n_knots != n_control + degree + 1) { + throw std::invalid_argument( + "Invalid knot vector length for the given degree and number of control points. " + "Expected " + + std::to_string(n_control + degree + 1) + ", got " + std::to_string(n_knots) + + ". The relationship must satisfy: n_knots = n_control_points + degree + 1"); + } + + knots_ = Eigen::Map(knots.data(), n_knots); + u_min_ = knots_[degree]; + u_max_ = knots_[n_knots - degree - 1]; + dimension_ = static_cast(control_points.cols()); +} + +int BSpline::find_knot_span(double u) const { + // Validate parameter range + if (u < u_min_ - kEps || u > u_max_ + kEps) { + throw std::invalid_argument("Parameter u=" + std::to_string(u) + + " outside valid range [" + std::to_string(u_min_) + ", " + + std::to_string(u_max_) + "]"); + } + + // Clamp to valid range + u = std::clamp(u, u_min_, u_max_); + + // Handle endpoint case + if (std::abs(u - u_max_) <= kEps) { + return static_cast(knots_.size()) - degree_ - 2; + } + + // Binary search + const int n = static_cast(control_points_.rows()) - 1; + int low = degree_; + int high = n + 1; + + while (low < high - 1) { + int mid = (low + high) / 2; + if (u >= knots_[mid]) { + low = mid; + } else { + high = mid; + } + } + + return low; +} + +Eigen::VectorXd BSpline::basis_functions(double u, int span_index) const { + const int p = degree_; + Eigen::VectorXd n_basis = Eigen::VectorXd::Zero(p + 1); + Eigen::VectorXd left = Eigen::VectorXd::Zero(p + 1); + Eigen::VectorXd right = Eigen::VectorXd::Zero(p + 1); + + n_basis[0] = 1.0; + + for (int d = 1; d <= p; ++d) { + left[d] = u - knots_[span_index + 1 - d]; + right[d] = knots_[span_index + d] - u; + + double saved = 0.0; + + for (int r = 0; r < d; ++r) { + double denominator = right[r + 1] + left[d - r]; + double temp = (std::abs(denominator) < kEps) ? 0.0 : n_basis[r] / denominator; + n_basis[r] = saved + right[r + 1] * temp; + saved = left[d - r] * temp; + } + + n_basis[d] = saved; + } + + return n_basis; +} + +Eigen::VectorXd BSpline::evaluate(double u) const { + // Handle endpoints exactly + if (std::abs(u - u_min_) <= kEps) { + return control_points_.row(0).transpose(); + } + if (std::abs(u - u_max_) <= kEps) { + return control_points_.row(control_points_.rows() - 1).transpose(); + } + + u = std::clamp(u, u_min_, u_max_); + + const int span = find_knot_span(u); + const Eigen::VectorXd n_basis = basis_functions(u, span); + + Eigen::VectorXd point = Eigen::VectorXd::Zero(dimension_); + for (int i = 0; i <= degree_; ++i) { + point += n_basis[i] * control_points_.row(span - degree_ + i).transpose(); + } + + return point; +} + +Eigen::MatrixXd BSpline::basis_function_derivatives(double u, int span_index, + int order) const { + const int p = degree_; + order = std::min(order, p); + + Eigen::MatrixXd ders = Eigen::MatrixXd::Zero(order + 1, p + 1); + Eigen::VectorXd left = Eigen::VectorXd::Zero(p + 1); + Eigen::VectorXd right = Eigen::VectorXd::Zero(p + 1); + Eigen::MatrixXd a = Eigen::MatrixXd::Zero(2, p + 1); + Eigen::MatrixXd ndu = Eigen::MatrixXd::Zero(p + 1, p + 1); + + ndu(0, 0) = 1.0; + + for (int j = 1; j <= p; ++j) { + left[j] = u - knots_[span_index + 1 - j]; + right[j] = knots_[span_index + j] - u; + double saved = 0.0; + + for (int r = 0; r < j; ++r) { + ndu(j, r) = right[r + 1] + left[j - r]; + double temp = (std::abs(ndu(j, r)) < kEps) ? 0.0 : ndu(r, j - 1) / ndu(j, r); + ndu(r, j) = saved + right[r + 1] * temp; + saved = left[j - r] * temp; + } + ndu(j, j) = saved; + } + + // Load basis functions + for (int j = 0; j <= p; ++j) { + ders(0, j) = ndu(j, p); + } + + // Calculate derivatives + for (int r = 0; r <= p; ++r) { + int s1 = 0; + int s2 = 1; + a(0, 0) = 1.0; + + for (int k = 1; k <= order; ++k) { + double d = 0.0; + int rk = r - k; + int pk = p - k; + + if (r >= k) { + a(s2, 0) = a(s1, 0) / ndu(pk + 1, rk); + d = a(s2, 0) * ndu(rk, pk); + } + + int j1 = (rk >= -1) ? 1 : -rk; + int j2 = (r - 1 <= pk) ? k - 1 : p - r; + + for (int j = j1; j <= j2; ++j) { + a(s2, j) = (a(s1, j) - a(s1, j - 1)) / ndu(pk + 1, rk + j); + d += a(s2, j) * ndu(rk + j, pk); + } + + if (r <= pk) { + a(s2, k) = -a(s1, k - 1) / ndu(pk + 1, r); + d += a(s2, k) * ndu(r, pk); + } + + ders(k, r) = d; + std::swap(s1, s2); + } + } + + // Multiply by correct factors: p!/(p-k)! + double r_factor = static_cast(p); + for (int k = 1; k <= order; ++k) { + for (int j = 0; j <= p; ++j) { + ders(k, j) *= r_factor; + } + r_factor *= static_cast(p - k); + } + + return ders; +} + +Eigen::VectorXd BSpline::evaluate_derivative(double u, int order) const { + if (order > degree_) { + throw std::invalid_argument("Derivative order " + std::to_string(order) + + " exceeds B-spline degree " + std::to_string(degree_)); + } + + if (order == 0) { + return evaluate(u); + } + + u = std::clamp(u, u_min_ + kEps, u_max_ - kEps); + + const int span = find_knot_span(u); + const Eigen::MatrixXd ders = basis_function_derivatives(u, span, order); + + Eigen::VectorXd derivative = Eigen::VectorXd::Zero(dimension_); + for (int j = 0; j <= degree_; ++j) { + derivative += ders(order, j) * control_points_.row(span - degree_ + j).transpose(); + } + + return derivative; +} + +std::pair +BSpline::generate_curve_points(int num_points) const { + Eigen::VectorXd u_values = Eigen::VectorXd::LinSpaced(num_points, u_min_, u_max_); + Eigen::MatrixXd curve_points(num_points, dimension_); + + for (int i = 0; i < num_points; ++i) { + curve_points.row(i) = evaluate(u_values[i]).transpose(); + } + + return {u_values, curve_points}; +} + +Eigen::VectorXd BSpline::create_uniform_knots(int degree, int num_control_points, + double domain_min, double domain_max) { + if (degree < 0) { + throw std::invalid_argument("Degree must be non-negative"); + } + if (num_control_points <= degree) { + throw std::invalid_argument( + "Number of control points must be greater than the degree"); + } + + const int num_knots = num_control_points + degree + 1; + Eigen::VectorXd knots = Eigen::VectorXd::Zero(num_knots); + + const int n_internal = num_knots - 2 * (degree + 1); + + // First degree+1 knots + for (int i = 0; i <= degree; ++i) { + knots[i] = domain_min; + } + + // Internal knots + if (n_internal >= 0) { + Eigen::VectorXd internal_vals = + Eigen::VectorXd::LinSpaced(n_internal + 2, domain_min, domain_max); + for (int i = 0; i < n_internal; ++i) { + knots[degree + 1 + i] = internal_vals[i + 1]; + } + } + + // Last degree+1 knots + for (int i = 0; i <= degree; ++i) { + knots[num_knots - degree - 1 + i] = domain_max; + } + + return knots; +} + +Eigen::VectorXd BSpline::create_periodic_knots(int degree, int num_control_points, + double domain_min, double domain_max) { + if (degree < 0) { + throw std::invalid_argument("Degree must be non-negative"); + } + if (num_control_points < degree + 1) { + throw std::invalid_argument( + "For a periodic B-spline, number of control points must be at least degree+1"); + } + + const int num_knots = num_control_points + degree + 1; + const int n_regular = num_knots - 2 * degree; + Eigen::VectorXd regular_knots = Eigen::VectorXd::LinSpaced(n_regular, domain_min, domain_max); + + Eigen::VectorXd knots = Eigen::VectorXd::Zero(num_knots); + const double step = (domain_max - domain_min) / static_cast(n_regular - 1); + + // Leading extension + for (int i = 0; i < degree; ++i) { + knots[i] = domain_min - static_cast(degree - i) * step; + } + + // Regular knots + for (int i = 0; i < n_regular; ++i) { + knots[degree + i] = regular_knots[i]; + } + + // Trailing extension + for (int i = 0; i < degree; ++i) { + knots[degree + n_regular + i] = domain_max + static_cast(i + 1) * step; + } + + return knots; +} + +} // namespace interpolatecpp::bspline diff --git a/cpp/src/bspline_interpolator.cpp b/cpp/src/bspline_interpolator.cpp new file mode 100644 index 0000000..bbb105f --- /dev/null +++ b/cpp/src/bspline_interpolator.cpp @@ -0,0 +1,261 @@ +#include + +#include +#include +#include +#include + +namespace interpolatecpp::bspline { + +namespace { +constexpr int kCubicDegree = 3; +constexpr int kQuarticDegree = 4; +constexpr int kQuinticDegree = 5; +} // namespace + +Eigen::VectorXd BSplineInterpolator::create_knot_vector(int degree, + const Eigen::MatrixXd& points, + const Eigen::VectorXd& times) { + const int n = static_cast(points.rows()) - 1; + const int p = degree; + + if (p % 2 == 1) { + // Odd degree: knots at interpolation points (eq. 4.42) + const int num_knots = n + 2 * p + 1; + Eigen::VectorXd knots = Eigen::VectorXd::Zero(num_knots); + + for (int i = 0; i <= p; ++i) { + knots[i] = times[0]; + } + + for (int i = 0; i < n - 1; ++i) { + knots[p + 1 + i] = times[1 + i]; + } + + for (int i = p + n; i < num_knots; ++i) { + knots[i] = times[n]; + } + + return knots; + } + + // Even degree: knots at midpoints (eq. 4.43) + const int num_knots = n + 2 * p + 2; + Eigen::VectorXd knots = Eigen::VectorXd::Zero(num_knots); + + for (int i = 0; i <= p; ++i) { + knots[i] = times[0]; + } + + for (int i = 0; i < n; ++i) { + knots[p + 1 + i] = (times[i] + times[i + 1]) / 2.0; + } + + for (int i = p + 1 + n; i < num_knots; ++i) { + knots[i] = times[n]; + } + + return knots; +} + +Eigen::MatrixXd BSplineInterpolator::compute_control_points( + int degree, const Eigen::MatrixXd& points, const Eigen::VectorXd& times, + const Eigen::VectorXd& knots) const { + const int n = static_cast(points.rows()) - 1; + const int p = degree; + const int dim = static_cast(points.cols()); + + const int num_cp = (p % 2 == 1) ? (n + 1 + p - 1) : (n + 1 + p); + const int num_additional = (p % 2 == 0) ? p : (p - 1); + const int total_rows = n + 1 + num_additional; + + // Create temporary BSpline for basis function calculation + Eigen::MatrixXd temp_ctrl = Eigen::MatrixXd::Zero(num_cp, 1); + BSpline temp_spline(degree, std::span(knots.data(), knots.size()), + temp_ctrl); + + // Build linear system A * P = b + Eigen::MatrixXd a_matrix = Eigen::MatrixXd::Zero(total_rows, num_cp); + Eigen::MatrixXd b = Eigen::MatrixXd::Zero(total_rows, dim); + + // Interpolation conditions + for (int i = 0; i <= n; ++i) { + const double t = times[i]; + const int span = temp_spline.find_knot_span(t); + const Eigen::VectorXd basis_vals = temp_spline.basis_functions(t, span); + + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(i, col) = basis_vals[j]; + } + } + b.row(i) = points.row(i); + } + + // Boundary conditions + int row = n + 1; + + if (cyclic_) { + for (int k = 1; k <= num_additional && row < total_rows; ++k) { + const int span0 = temp_spline.find_knot_span(times[0]); + const int spann = temp_spline.find_knot_span(times[n]); + const Eigen::MatrixXd ders0 = + temp_spline.basis_function_derivatives(times[0], span0, k); + const Eigen::MatrixXd dersn = + temp_spline.basis_function_derivatives(times[n], spann, k); + + for (int j = 0; j <= p; ++j) { + int col0 = span0 - p + j; + if (col0 >= 0 && col0 < num_cp) { + a_matrix(row, col0) = ders0(k, j); + } + int coln = spann - p + j; + if (coln >= 0 && coln < num_cp) { + a_matrix(row, coln) -= dersn(k, j); + } + } + ++row; + } + } else { + // Velocity constraints + if (initial_velocity_.has_value() && row < total_rows) { + const int span = temp_spline.find_knot_span(times[0]); + const Eigen::MatrixXd ders = + temp_spline.basis_function_derivatives(times[0], span, 1); + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(row, col) = ders(1, j); + } + } + b.row(row) = initial_velocity_.value().transpose(); + ++row; + } + + if (final_velocity_.has_value() && row < total_rows) { + const int span = temp_spline.find_knot_span(times[n]); + const Eigen::MatrixXd ders = + temp_spline.basis_function_derivatives(times[n], span, 1); + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(row, col) = ders(1, j); + } + } + b.row(row) = final_velocity_.value().transpose(); + ++row; + } + + // Acceleration constraints + if (initial_acceleration_.has_value() && row < total_rows) { + const int span = temp_spline.find_knot_span(times[0]); + const Eigen::MatrixXd ders = + temp_spline.basis_function_derivatives(times[0], span, 2); + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(row, col) = ders(2, j); + } + } + b.row(row) = initial_acceleration_.value().transpose(); + ++row; + } + + if (final_acceleration_.has_value() && row < total_rows) { + const int span = temp_spline.find_knot_span(times[n]); + const Eigen::MatrixXd ders = + temp_spline.basis_function_derivatives(times[n], span, 2); + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(row, col) = ders(2, j); + } + } + b.row(row) = final_acceleration_.value().transpose(); + ++row; + } + + // Fill remaining rows with natural spline conditions + while (row < total_rows) { + int deriv_order = std::min(p - 1, 2); + double t = (row % 2 == 0) ? times[0] : times[n]; + + const int span = temp_spline.find_knot_span(t); + const Eigen::MatrixXd ders = + temp_spline.basis_function_derivatives(t, span, deriv_order); + + for (int j = 0; j <= p; ++j) { + int col = span - p + j; + if (col >= 0 && col < num_cp) { + a_matrix(row, col) = ders(deriv_order, j); + } + } + ++row; + } + } + + // Solve using ColPivHouseholderQR (handles rank-deficient systems gracefully) + Eigen::ColPivHouseholderQR solver(a_matrix); + solver.setThreshold(kRegularizationEps); + + Eigen::MatrixXd cp(num_cp, dim); + for (int d = 0; d < dim; ++d) { + cp.col(d) = solver.solve(b.col(d)); + } + + return cp; +} + +BSplineInterpolator::BSplineInterpolator( + int degree, const Eigen::MatrixXd& points, + const std::optional& times, + const std::optional& initial_velocity, + const std::optional& final_velocity, + const std::optional& initial_acceleration, + const std::optional& final_acceleration, bool cyclic) + : BSpline(DeferInit{}) +{ + if (degree != kCubicDegree && degree != kQuarticDegree && degree != kQuinticDegree) { + throw std::invalid_argument("Degree must be 3, 4, or 5, got " + + std::to_string(degree)); + } + + Eigen::MatrixXd pts = points; + const int num_points = static_cast(pts.rows()); + const int min_points = degree + 1; + + if (num_points < min_points) { + throw std::invalid_argument( + "Not enough points for degree " + std::to_string(degree) + + " B-spline interpolation. Need at least " + std::to_string(min_points) + + " points, but got " + std::to_string(num_points) + "."); + } + + interp_points_ = pts; + initial_velocity_ = initial_velocity; + final_velocity_ = final_velocity; + initial_acceleration_ = initial_acceleration; + final_acceleration_ = final_acceleration; + cyclic_ = cyclic; + + if (times.has_value()) { + times_ = times.value(); + } else { + times_ = Eigen::VectorXd::LinSpaced(num_points, 0.0, + static_cast(num_points - 1)); + } + + Eigen::VectorXd new_knots = create_knot_vector(degree, pts, times_); + Eigen::MatrixXd new_cp = compute_control_points(degree, pts, times_, new_knots); + + // Reinitialize base class + degree_ = degree; + knots_ = new_knots; + control_points_ = new_cp; + u_min_ = knots_[degree]; + u_max_ = knots_[static_cast(knots_.size()) - degree - 1]; + dimension_ = static_cast(pts.cols()); +} + +} // namespace interpolatecpp::bspline diff --git a/cpp/src/circular_path.cpp b/cpp/src/circular_path.cpp new file mode 100644 index 0000000..f6952b0 --- /dev/null +++ b/cpp/src/circular_path.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::path { + +CircularPath::CircularPath(const Eigen::Vector3d& axis, + const Eigen::Vector3d& axis_point, + const Eigen::Vector3d& circle_point) { + axis_ = axis.normalized(); + + Eigen::Vector3d delta = circle_point - axis_point; + + if (std::abs(delta.dot(axis_)) >= delta.norm()) { + throw std::invalid_argument("The point must not be on the circle axis"); + } + + // Center = projection of circle_point onto axis line + center_ = axis_point + delta.dot(axis_) * axis_; + radius_ = (circle_point - center_).norm(); + + // Rotation matrix: local (x',y',z') -> global + Eigen::Vector3d x_prime = (circle_point - center_) / radius_; + Eigen::Vector3d z_prime = axis_; + Eigen::Vector3d y_prime = z_prime.cross(x_prime); + + R_.col(0) = x_prime; + R_.col(1) = y_prime; + R_.col(2) = z_prime; +} + +Eigen::Vector3d CircularPath::position(double s) const { + double angle = s / radius_; + Eigen::Vector3d p_local(radius_ * std::cos(angle), radius_ * std::sin(angle), 0.0); + return center_ + R_ * p_local; +} + +Eigen::MatrixXd CircularPath::position(const Eigen::VectorXd& s) const { + Eigen::MatrixXd result(s.size(), 3); + for (Eigen::Index i = 0; i < s.size(); ++i) { + result.row(i) = position(s[i]).transpose(); + } + return result; +} + +Eigen::Vector3d CircularPath::velocity(double s) const { + double angle = s / radius_; + Eigen::Vector3d dp_local(-std::sin(angle), std::cos(angle), 0.0); + return R_ * dp_local; +} + +Eigen::Vector3d CircularPath::acceleration(double s) const { + double angle = s / radius_; + Eigen::Vector3d d2p_local(-std::cos(angle) / radius_, -std::sin(angle) / radius_, + 0.0); + return R_ * d2p_local; +} + +} // namespace interpolatecpp::path diff --git a/cpp/src/cubic_bspline_interpolation.cpp b/cpp/src/cubic_bspline_interpolation.cpp new file mode 100644 index 0000000..aca38f1 --- /dev/null +++ b/cpp/src/cubic_bspline_interpolation.cpp @@ -0,0 +1,213 @@ +#include +#include + +#include +#include + +namespace interpolatecpp::bspline { + +Eigen::VectorXd CubicBSplineInterpolation::calculate_parameters( + const Eigen::MatrixXd& points, Parameterization method) { + const int n = static_cast(points.rows()) - 1; + Eigen::VectorXd u_bars = Eigen::VectorXd::Zero(n + 1); + u_bars[0] = 0.0; + u_bars[n] = 1.0; + + switch (method) { + case Parameterization::EquallySpaced: { + for (int k = 1; k < n; ++k) { + u_bars[k] = static_cast(k) / static_cast(n); + } + break; + } + case Parameterization::ChordLength: { + double total_length = 0.0; + for (int k = 1; k <= n; ++k) { + total_length += (points.row(k) - points.row(k - 1)).norm(); + } + for (int k = 1; k < n; ++k) { + u_bars[k] = u_bars[k - 1] + + (points.row(k) - points.row(k - 1)).norm() / total_length; + } + break; + } + case Parameterization::Centripetal: { + constexpr double mu = 0.5; + double total_length = 0.0; + for (int k = 1; k <= n; ++k) { + total_length += std::pow((points.row(k) - points.row(k - 1)).norm(), mu); + } + for (int k = 1; k < n; ++k) { + u_bars[k] = + u_bars[k - 1] + + std::pow((points.row(k) - points.row(k - 1)).norm(), mu) / total_length; + } + break; + } + } + + return u_bars; +} + +Eigen::VectorXd CubicBSplineInterpolation::calculate_knot_vector( + const Eigen::VectorXd& u_bars, int n_points) { + const int n = n_points - 1; + Eigen::VectorXd knots = Eigen::VectorXd::Zero(n + 7); + + // First 3 knots = u_bars[0] + knots[0] = u_bars[0]; + knots[1] = u_bars[0]; + knots[2] = u_bars[0]; + + // Middle knots: u_j+3 = u_bars[j] for j = 0, ..., n + for (int j = 0; j <= n; ++j) { + knots[j + 3] = u_bars[j]; + } + + // Last 3 knots = u_bars[n] + knots[n + 4] = u_bars[n]; + knots[n + 5] = u_bars[n]; + knots[n + 6] = u_bars[n]; + + return knots; +} + +Eigen::MatrixXd CubicBSplineInterpolation::calculate_control_points( + const Eigen::VectorXd& knots) const { + const int n = static_cast(interpolation_points_.rows()) - 1; + const int dim = static_cast(interpolation_points_.cols()); + + Eigen::MatrixXd cp = Eigen::MatrixXd::Zero(n + 3, dim); + + // Direct control points from endpoints and derivatives (eq. 8.16) + cp.row(0) = interpolation_points_.row(0); + cp.row(1) = interpolation_points_.row(0) + (knots[4] / 3.0) * v0_.transpose(); + cp.row(n + 1) = + interpolation_points_.row(n) - ((1.0 - knots[n + 2]) / 3.0) * vn_.transpose(); + cp.row(n + 2) = interpolation_points_.row(n); + + if (n < kMinPointsForTridiagonal) { + return cp; + } + + // Solve tridiagonal system for remaining control points + const int sys_size = n - 1; + Eigen::VectorXd lower_diag = Eigen::VectorXd::Zero(sys_size); + Eigen::VectorXd main_diag = Eigen::VectorXd::Zero(sys_size); + Eigen::VectorXd upper_diag = Eigen::VectorXd::Zero(sys_size); + Eigen::MatrixXd rhs = Eigen::MatrixXd::Zero(sys_size, dim); + + // Create temporary BSpline for basis function calculation + Eigen::MatrixXd temp_ctrl = Eigen::MatrixXd::Zero(n + 3, dim); + BSpline temp_bs(3, std::span(knots.data(), knots.size()), temp_ctrl); + + for (int i = 0; i < sys_size; ++i) { + const int k = i + 1; // Point index (1 to n-1) + const double u_bar = u_bars_[k]; + const int span = temp_bs.find_knot_span(u_bar); + const Eigen::VectorXd basis_vals = temp_bs.basis_functions(u_bar, span); + + const double b3_k = basis_vals[0]; + const double b3_k1 = basis_vals[1]; + const double b3_k2 = basis_vals[2]; + + if (k == 1) { + main_diag[0] = b3_k1; + upper_diag[0] = b3_k2; + rhs.row(0) = interpolation_points_.row(k) - b3_k * cp.row(1); + } else if (k == n - 1) { + lower_diag[k - 2] = b3_k; + main_diag[k - 1] = b3_k1; + rhs.row(k - 1) = interpolation_points_.row(k) - b3_k2 * cp.row(n + 1); + } else { + lower_diag[k - 2] = b3_k; + main_diag[k - 1] = b3_k1; + upper_diag[k - 1] = b3_k2; + rhs.row(k - 1) = interpolation_points_.row(k); + } + } + + // Solve per dimension using tridiagonal solver + for (int d = 0; d < dim; ++d) { + Eigen::VectorXd l_diag = Eigen::VectorXd::Zero(sys_size); + if (sys_size > 1) { + l_diag.tail(sys_size - 1) = lower_diag.head(sys_size - 1); + } + + Eigen::VectorXd solution = + interpolatecpp::solve_tridiagonal(l_diag, main_diag, upper_diag, rhs.col(d)); + cp.block(2, d, sys_size, 1) = solution; + } + + return cp; +} + +CubicBSplineInterpolation::CubicBSplineInterpolation( + const Eigen::MatrixXd& points, const std::optional& v0, + const std::optional& vn, Parameterization method, + bool auto_derivatives) + : BSpline(DeferInit{}) +{ + // Ensure 2D points + Eigen::MatrixXd pts = points; + if (pts.cols() == 0) { + throw std::invalid_argument("Points array is empty"); + } + + const int n_points = static_cast(pts.rows()); + const int dim = static_cast(pts.cols()); + const int n = n_points - 1; + + interpolation_points_ = pts; + u_bars_ = calculate_parameters(pts, method); + + // Process v0 + if (v0.has_value()) { + if (v0->size() != dim) { + throw std::invalid_argument("v0 must be a vector of dimension " + + std::to_string(dim)); + } + v0_ = v0.value(); + } else if (auto_derivatives && n > 0) { + double u_diff = u_bars_[1] - u_bars_[0]; + if (std::abs(u_diff) > kParamDiffThreshold) { + v0_ = (pts.row(1) - pts.row(0)).transpose() / u_diff; + } else { + v0_ = Eigen::VectorXd::Zero(dim); + } + } else { + v0_ = Eigen::VectorXd::Zero(dim); + } + + // Process vn + if (vn.has_value()) { + if (vn->size() != dim) { + throw std::invalid_argument("vn must be a vector of dimension " + + std::to_string(dim)); + } + vn_ = vn.value(); + } else if (auto_derivatives && n > 0) { + double u_diff = u_bars_[n] - u_bars_[n - 1]; + if (std::abs(u_diff) > kParamDiffThreshold) { + vn_ = (pts.row(n) - pts.row(n - 1)).transpose() / u_diff; + } else { + vn_ = Eigen::VectorXd::Zero(dim); + } + } else { + vn_ = Eigen::VectorXd::Zero(dim); + } + + // Calculate knots and control points + Eigen::VectorXd new_knots = calculate_knot_vector(u_bars_, n_points); + Eigen::MatrixXd new_cp = calculate_control_points(new_knots); + + // Reinitialize base class with computed values + degree_ = 3; + knots_ = new_knots; + control_points_ = new_cp; + u_min_ = knots_[3]; + u_max_ = knots_[static_cast(knots_.size()) - 4]; + dimension_ = dim; +} + +} // namespace interpolatecpp::bspline diff --git a/cpp/src/cubic_smoothing_spline.cpp b/cpp/src/cubic_smoothing_spline.cpp new file mode 100644 index 0000000..8136040 --- /dev/null +++ b/cpp/src/cubic_smoothing_spline.cpp @@ -0,0 +1,270 @@ +#include + +#include +#include +#include +#include +#include + +namespace interpolatecpp::spline { + +CubicSmoothingSpline::CubicSmoothingSpline(std::span t_points, + std::span q_points, double mu, + std::optional> weights, + double v0, double vn, bool debug) + : mu_(mu), v0_(v0), vn_(vn), debug_(debug) { + if (t_points.size() != q_points.size()) { + throw std::invalid_argument("Time and position arrays must have the same length"); + } + + n_ = static_cast(t_points.size()); + + if (n_ < kMinPointsRequired) { + throw std::invalid_argument("At least two points are required"); + } + + t_ = Eigen::Map(t_points.data(), n_); + q_ = Eigen::Map(q_points.data(), n_); + + // Check strictly increasing + for (int i = 1; i < n_; ++i) { + if (t_(i) <= t_(i - 1)) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + if (mu_ <= 0.0 || mu_ > 1.0) { + throw std::invalid_argument("Parameter mu must be in range (0, 1]"); + } + + // Compute lambda + lambd_ = (mu_ < 1.0) ? (1.0 - mu_) / (6.0 * mu_ + 1e-10) : 0.0; + + // Compute time intervals + time_intervals_ = t_.tail(n_ - 1) - t_.head(n_ - 1); + + // Initialize weights + if (weights.has_value()) { + auto w_span = weights.value(); + if (static_cast(w_span.size()) != n_) { + throw std::invalid_argument( + "Weights array must have the same length as time and position arrays"); + } + w_ = Eigen::Map(w_span.data(), n_); + } else { + w_ = Eigen::VectorXd::Ones(n_); + } + + // Handle infinite weights + w_inv_ = Eigen::VectorXd::Zero(n_); + for (int i = 0; i < n_; ++i) { + if (std::isfinite(w_(i))) { + w_inv_(i) = 1.0 / w_(i); + } + } + + if (debug_) { + std::cout << "Smoothing parameter mu: " << mu_ << "\n"; + std::cout << "Lambda: " << lambd_ << "\n"; + std::cout << "Weights: " << w_.transpose() << "\n"; + std::cout << "Inverse weights: " << w_inv_.transpose() << "\n"; + } + + construct_matrices(); + solve_system(); + compute_positions(); + compute_coefficients(); + + if (debug_) { + std::cout << "Original points: " << q_.transpose() << "\n"; + std::cout << "Approximated points: " << s_.transpose() << "\n"; + std::cout << "Accelerations: " << omega_.transpose() << "\n"; + std::cout << "Maximum position error: " << (q_ - s_).cwiseAbs().maxCoeff() << "\n"; + } +} + +void CubicSmoothingSpline::construct_matrices() { + a_matrix_ = Eigen::MatrixXd::Zero(n_, n_); + c_matrix_ = Eigen::MatrixXd::Zero(n_, n_); + + // Fill A matrix (equation 4.23) + a_matrix_(0, 0) = 2.0 * time_intervals_(0); + a_matrix_(n_ - 1, n_ - 1) = 2.0 * time_intervals_(n_ - 2); + + for (int i = 1; i < n_ - 1; ++i) { + a_matrix_(i, i) = 2.0 * (time_intervals_(i - 1) + time_intervals_(i)); + } + + for (int i = 0; i < n_ - 1; ++i) { + a_matrix_(i, i + 1) = time_intervals_(i); + a_matrix_(i + 1, i) = time_intervals_(i); + } + + // Fill C matrix (equation 4.34) + c_matrix_(0, 0) = -6.0 / time_intervals_(0); + c_matrix_(0, 1) = 6.0 / time_intervals_(0); + c_matrix_(n_ - 1, n_ - 2) = 6.0 / time_intervals_(n_ - 2); + c_matrix_(n_ - 1, n_ - 1) = -6.0 / time_intervals_(n_ - 2); + + for (int i = 1; i < n_ - 1; ++i) { + c_matrix_(i, i - 1) = 6.0 / time_intervals_(i - 1); + c_matrix_(i, i) = -(6.0 / time_intervals_(i - 1) + 6.0 / time_intervals_(i)); + c_matrix_(i, i + 1) = 6.0 / time_intervals_(i); + } + + if (debug_) { + std::cout << "Matrix A:\n" << a_matrix_ << "\n"; + std::cout << "Matrix C:\n" << c_matrix_ << "\n"; + } +} + +void CubicSmoothingSpline::solve_system() { + if (mu_ == 1.0) { + // Pure interpolation: A*omega = c (equation 4.22/4.24) + Eigen::VectorXd c = Eigen::VectorXd::Zero(n_); + + c(0) = 6.0 * ((q_(1) - q_(0)) / time_intervals_(0) - v0_); + c(n_ - 1) = 6.0 * (vn_ - (q_(n_ - 1) - q_(n_ - 2)) / time_intervals_(n_ - 2)); + + for (int i = 1; i < n_ - 1; ++i) { + c(i) = 6.0 * ((q_(i + 1) - q_(i)) / time_intervals_(i) - + (q_(i) - q_(i - 1)) / time_intervals_(i - 1)); + } + + if (debug_) { + std::cout << "Vector c (pure interpolation): " << c.transpose() << "\n"; + } + + omega_ = a_matrix_.colPivHouseholderQr().solve(c); + return; + } + + // Smoothing spline: (A + lambda*C*W_inv*C^T)*omega = C*q + Eigen::VectorXd rhs = c_matrix_ * q_; + Eigen::MatrixXd w_inv_diag = w_inv_.asDiagonal(); + Eigen::MatrixXd c_w_inv_ct = c_matrix_ * w_inv_diag * c_matrix_.transpose(); + + // Symmetrize + c_w_inv_ct = (c_w_inv_ct + c_w_inv_ct.transpose()) / 2.0; + + Eigen::MatrixXd system_matrix = a_matrix_ + lambd_ * c_w_inv_ct; + system_matrix = (system_matrix + system_matrix.transpose()) / 2.0; + + if (debug_) { + std::cout << "System matrix (smoothing):\n" << system_matrix << "\n"; + std::cout << "RHS vector: " << rhs.transpose() << "\n"; + } + + // Check condition number and regularize if needed + Eigen::JacobiSVD svd(system_matrix); + double cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + + if (cond > kHighConditionThreshold) { + system_matrix += Eigen::MatrixXd::Identity(n_, n_) * kRegularizationFactor; + if (debug_) { + std::cout << "Added regularization. New condition number will be lower.\n"; + } + } + + omega_ = system_matrix.colPivHouseholderQr().solve(rhs); +} + +void CubicSmoothingSpline::compute_positions() { + if (mu_ == 1.0) { + s_ = q_; + return; + } + + Eigen::VectorXd ct_omega = c_matrix_.transpose() * omega_; + Eigen::VectorXd adjustment = lambd_ * (w_inv_.array() * ct_omega.array()).matrix(); + s_ = q_ - adjustment; + + if (debug_) { + std::cout << "Computed s: " << s_.transpose() << " for mu: " << mu_ << "\n"; + } +} + +void CubicSmoothingSpline::compute_coefficients() { + int n_segments = n_ - 1; + coeffs_.resize(n_segments, 4); + + for (int k = 0; k < n_segments; ++k) { + double T = time_intervals_(k); + + coeffs_(k, 0) = s_(k); + coeffs_(k, 1) = (s_(k + 1) - s_(k)) / T - (T / 6.0) * (omega_(k + 1) + 2.0 * omega_(k)); + coeffs_(k, 2) = omega_(k) / 2.0; + coeffs_(k, 3) = (omega_(k + 1) - omega_(k)) / (6.0 * T); + } + + if (debug_) { + std::cout << "Polynomial coefficients:\n"; + for (int k = 0; k < n_segments; ++k) { + std::cout << "Segment " << k << ": " << coeffs_.row(k) << "\n"; + } + } +} + +CubicSmoothingSpline::SegmentInfo CubicSmoothingSpline::find_segment(double t) const { + if (t <= t_(0)) { + return {0, t - t_(0)}; + } + if (t >= t_(n_ - 1)) { + return {n_ - 2, t - t_(n_ - 2)}; + } + auto begin = t_.data(); + auto end = begin + n_; + auto it = std::upper_bound(begin, end, t); + int k = static_cast(it - begin) - 1; + return {k, t - t_(k)}; +} + +double CubicSmoothingSpline::evaluate(double t) const { + auto [k, tau] = find_segment(t); + double c0 = coeffs_(k, 0); + double c1 = coeffs_(k, 1); + double c2 = coeffs_(k, 2); + double c3 = coeffs_(k, 3); + return c0 + c1 * tau + c2 * tau * tau + c3 * tau * tau * tau; +} + +Eigen::VectorXd CubicSmoothingSpline::evaluate(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate(t(i)); + } + return result; +} + +double CubicSmoothingSpline::evaluate_velocity(double t) const { + auto [k, tau] = find_segment(t); + double c1 = coeffs_(k, 1); + double c2 = coeffs_(k, 2); + double c3 = coeffs_(k, 3); + return c1 + 2.0 * c2 * tau + 3.0 * c3 * tau * tau; +} + +Eigen::VectorXd CubicSmoothingSpline::evaluate_velocity(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_velocity(t(i)); + } + return result; +} + +double CubicSmoothingSpline::evaluate_acceleration(double t) const { + auto [k, tau] = find_segment(t); + double c2 = coeffs_(k, 2); + double c3 = coeffs_(k, 3); + return 2.0 * c2 + 6.0 * c3 * tau; +} + +Eigen::VectorXd CubicSmoothingSpline::evaluate_acceleration(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_acceleration(t(i)); + } + return result; +} + +} // namespace interpolatecpp::spline diff --git a/cpp/src/cubic_spline.cpp b/cpp/src/cubic_spline.cpp new file mode 100644 index 0000000..69d0cb0 --- /dev/null +++ b/cpp/src/cubic_spline.cpp @@ -0,0 +1,199 @@ +#include +#include + +#include +#include +#include +#include + +namespace interpolatecpp::spline { + +CubicSpline::CubicSpline(std::span t_points, std::span q_points, + double v0, double vn, bool debug) + : v0_(v0), vn_(vn), debug_(debug) { + if (t_points.size() != q_points.size()) { + throw std::invalid_argument("Time points and position points must have the same length"); + } + + const auto n_points = static_cast(t_points.size()); + if (n_points < 2) { + throw std::invalid_argument("At least two points are required"); + } + + t_points_ = Eigen::Map(t_points.data(), n_points); + q_points_ = Eigen::Map(q_points.data(), n_points); + + // Check strictly increasing + for (int i = 1; i < n_points; ++i) { + if (t_points_[i] <= t_points_[i - 1]) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + n_ = n_points - 1; + t_intervals_ = t_points_.tail(n_).array() - t_points_.head(n_).array(); + + compute_velocities(); + compute_coefficients(); +} + +void CubicSpline::compute_velocities() { + if (n_ == 1) { + velocities_.resize(2); + velocities_(0) = v0_; + velocities_(1) = vn_; + return; + } + + // Build RHS: c_i = 3/(T_i*T_{i+1}) * [T_i^2*(q_{i+2}-q_{i+1}) + T_{i+1}^2*(q_{i+1}-q_i)] + const int m = n_ - 1; // number of intermediate velocities + Eigen::VectorXd rhs(m); + + for (int i = 0; i < m; ++i) { + rhs(i) = 3.0 / (t_intervals_(i) * t_intervals_(i + 1)) * + (t_intervals_(i) * t_intervals_(i) * (q_points_(i + 2) - q_points_(i + 1)) + + t_intervals_(i + 1) * t_intervals_(i + 1) * (q_points_(i + 1) - q_points_(i))); + } + + // Adjust for boundary velocities + rhs(0) -= t_intervals_(1) * v0_; + rhs(m - 1) -= t_intervals_(n_ - 2) * vn_; + + Eigen::VectorXd v_intermediate; + + if (n_ == 2) { + // 1x1 system: simple division + double main_diag_value = 2.0 * (t_intervals_(0) + t_intervals_(1)); + v_intermediate = rhs / main_diag_value; + } else { + // Build tridiagonal diagonals + Eigen::VectorXd main_diag(m); + Eigen::VectorXd lower_diag = Eigen::VectorXd::Zero(m); + Eigen::VectorXd upper_diag = Eigen::VectorXd::Zero(m); + + for (int i = 0; i < m; ++i) { + main_diag(i) = 2.0 * (t_intervals_(i) + t_intervals_(i + 1)); + } + for (int i = 1; i < m; ++i) { + lower_diag(i) = t_intervals_(i + 1); + } + for (int i = 0; i < m - 1; ++i) { + upper_diag(i) = t_intervals_(i); + } + + if (debug_) { + std::cout << "\nTridiagonal Matrix components:\n"; + std::cout << "Main diagonal: " << main_diag.transpose() << "\n"; + std::cout << "Lower diagonal: " << lower_diag.transpose() << "\n"; + std::cout << "Upper diagonal: " << upper_diag.transpose() << "\n"; + std::cout << "Right-hand side vector: " << rhs.transpose() << "\n"; + } + + v_intermediate = solve_tridiagonal(lower_diag, main_diag, upper_diag, rhs); + + if (debug_) { + std::cout << "\nIntermediate velocities: " << v_intermediate.transpose() << "\n"; + } + } + + // Assemble full velocity vector + velocities_.resize(n_ + 1); + velocities_(0) = v0_; + velocities_.segment(1, m) = v_intermediate; + velocities_(n_) = vn_; + + if (debug_) { + std::cout << "\nComplete velocity vector: " << velocities_.transpose() << "\n"; + } +} + +void CubicSpline::compute_coefficients() { + coefficients_.resize(n_, 4); + + for (int k = 0; k < n_; ++k) { + double T = t_intervals_(k); + double q_k = q_points_(k); + double q_k1 = q_points_(k + 1); + double v_k = velocities_(k); + double v_k1 = velocities_(k + 1); + + coefficients_(k, 0) = q_k; // a0 + coefficients_(k, 1) = v_k; // a1 + coefficients_(k, 2) = (1.0 / T) * (3.0 * (q_k1 - q_k) / T - 2.0 * v_k - v_k1); // a2 + coefficients_(k, 3) = + (1.0 / (T * T)) * (2.0 * (q_k - q_k1) / T + v_k + v_k1); // a3 + + if (debug_) { + std::cout << "\nCoefficient calculation for segment " << k << ":\n"; + std::cout << " a0 = " << coefficients_(k, 0) << "\n"; + std::cout << " a1 = " << coefficients_(k, 1) << "\n"; + std::cout << " a2 = " << coefficients_(k, 2) << "\n"; + std::cout << " a3 = " << coefficients_(k, 3) << "\n"; + } + } +} + +CubicSpline::SegmentInfo CubicSpline::find_segment(double t) const { + if (t <= t_points_(0)) { + return {0, 0.0}; + } + if (t >= t_points_(n_)) { + return {n_ - 1, t_intervals_(n_ - 1)}; + } + // Binary search: find largest k such that t_points_[k] <= t + auto begin = t_points_.data(); + auto end = begin + n_ + 1; + auto it = std::upper_bound(begin, end, t); + int k = static_cast(it - begin) - 1; + return {k, t - t_points_(k)}; +} + +double CubicSpline::evaluate(double t) const { + auto [k, tau] = find_segment(t); + double a0 = coefficients_(k, 0); + double a1 = coefficients_(k, 1); + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return a0 + a1 * tau + a2 * tau * tau + a3 * tau * tau * tau; +} + +Eigen::VectorXd CubicSpline::evaluate(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate(t(i)); + } + return result; +} + +double CubicSpline::evaluate_velocity(double t) const { + auto [k, tau] = find_segment(t); + double a1 = coefficients_(k, 1); + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return a1 + 2.0 * a2 * tau + 3.0 * a3 * tau * tau; +} + +Eigen::VectorXd CubicSpline::evaluate_velocity(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_velocity(t(i)); + } + return result; +} + +double CubicSpline::evaluate_acceleration(double t) const { + auto [k, tau] = find_segment(t); + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return 2.0 * a2 + 6.0 * a3 * tau; +} + +Eigen::VectorXd CubicSpline::evaluate_acceleration(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_acceleration(t(i)); + } + return result; +} + +} // namespace interpolatecpp::spline diff --git a/cpp/src/cubic_spline_with_acc1.cpp b/cpp/src/cubic_spline_with_acc1.cpp new file mode 100644 index 0000000..eca1793 --- /dev/null +++ b/cpp/src/cubic_spline_with_acc1.cpp @@ -0,0 +1,280 @@ +#include +#include + +#include +#include +#include +#include + +namespace interpolatecpp::spline { + +namespace { +constexpr int kMinPoints = 2; +constexpr int kMinSegmentsForUpperDiag = 2; +constexpr int kMinSegmentsForSecondRow = 3; +constexpr int kMinSegmentsForUpperDiagSecondRow = 4; +constexpr int kMinSegmentsForSecondElement = 4; +} // namespace + +CubicSplineWithAcceleration1::CubicSplineWithAcceleration1(std::span t_points, + std::span q_points, + double v0, double vn, double a0, + double an, bool debug) + : v0_(v0), vn_(vn), a0_(a0), an_(an), debug_(debug) { + if (t_points.size() != q_points.size()) { + throw std::invalid_argument("Time and position arrays must have the same length"); + } + + n_orig_ = static_cast(t_points.size()); + if (n_orig_ < kMinPoints) { + throw std::invalid_argument("At least two points are required"); + } + + t_orig_ = Eigen::Map(t_points.data(), n_orig_); + q_orig_ = Eigen::Map(q_points.data(), n_orig_); + + for (int i = 1; i < n_orig_; ++i) { + if (t_orig_(i) <= t_orig_(i - 1)) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + add_extra_points(); + n_ = static_cast(t_.size()); + T_ = t_.tail(n_ - 1) - t_.head(n_ - 1); + + if (debug_) { + std::cout << "Time interval length: " << T_.transpose() << "\n\n"; + } + + solve_accelerations(); + compute_coefficients(); + compute_original_indices(); +} + +void CubicSplineWithAcceleration1::add_extra_points() { + int new_n = n_orig_ + 2; + t_.resize(new_n); + q_.resize(new_n); + + t_(0) = t_orig_(0); + t_(new_n - 1) = t_orig_(n_orig_ - 1); + q_(0) = q_orig_(0); + q_(new_n - 1) = q_orig_(n_orig_ - 1); + + // Interior original points + for (int i = 1; i < n_orig_ - 1; ++i) { + t_(i + 1) = t_orig_(i); + q_(i + 1) = q_orig_(i); + } + + // Extra points at midpoints + t_(1) = (t_orig_(0) + t_orig_(1)) / 2.0; + t_(new_n - 2) = (t_orig_(n_orig_ - 2) + t_orig_(n_orig_ - 1)) / 2.0; + + // q values for extra points will be computed after solving accelerations + q_(1) = 0.0; + q_(new_n - 2) = 0.0; + + if (debug_) { + std::cout << "Original times: " << t_orig_.transpose() << "\n"; + std::cout << "New times with extra points: " << t_.transpose() << "\n\n"; + } +} + +void CubicSplineWithAcceleration1::solve_accelerations() { + int n_segments = n_ - 1; + int sys_size = n_segments - 1; + + Eigen::VectorXd main_diag = Eigen::VectorXd::Zero(sys_size); + Eigen::VectorXd lower_diag = Eigen::VectorXd::Zero(sys_size); + Eigen::VectorXd upper_diag = Eigen::VectorXd::Zero(sys_size); + + // First element of main diagonal + main_diag(0) = 2.0 * T_(1) + T_(0) * (3.0 + T_(0) / T_(1)); + if (n_segments > kMinSegmentsForUpperDiag) { + upper_diag(0) = T_(1); + } + + // Last element of main diagonal + main_diag(sys_size - 1) = 2.0 * T_(n_segments - 2) + + T_(n_segments - 1) * + (3.0 + T_(n_segments - 1) / T_(n_segments - 2)); + if (n_segments > kMinSegmentsForUpperDiag) { + lower_diag(sys_size - 1) = T_(n_segments - 2); + } + + if (n_segments > kMinSegmentsForSecondRow) { + // Second row + lower_diag(1) = T_(1) - (T_(0) * T_(0) / T_(1)); + main_diag(1) = 2.0 * (T_(1) + T_(2)); + if (n_segments > kMinSegmentsForUpperDiagSecondRow) { + upper_diag(1) = T_(2); + } + + // Second-to-last row + main_diag(sys_size - 2) = + 2.0 * (T_(n_segments - 3) + T_(n_segments - 2)); + lower_diag(sys_size - 2) = T_(n_segments - 3); + upper_diag(sys_size - 2) = + T_(n_segments - 2) - + T_(n_segments - 1) * T_(n_segments - 1) / T_(n_segments - 2); + } + + // Middle rows + for (int i = 2; i < sys_size - 2; ++i) { + lower_diag(i) = T_(i); + main_diag(i) = 2.0 * (T_(i) + T_(i + 1)); + upper_diag(i) = T_(i + 1); + } + + // Construct RHS vector c (equation 4.28) + Eigen::VectorXd c = Eigen::VectorXd::Zero(sys_size); + + c(0) = 6.0 * ((q_(2) - q_(0)) / T_(1) - v0_ * (1.0 + T_(0) / T_(1)) - + a0_ * (0.5 + T_(0) / (3.0 * T_(1))) * T_(0)); + + c(sys_size - 1) = + 6.0 * ((q_(n_ - 3) - q_(n_ - 1)) / T_(n_segments - 2) + + vn_ * (1.0 + T_(n_segments - 1) / T_(n_segments - 2)) - + an_ * (0.5 + T_(n_segments - 1) / (3.0 * T_(n_segments - 2))) * + T_(n_segments - 1)); + + if (n_segments >= kMinSegmentsForSecondElement) { + c(1) = 6.0 * ((q_(3) - q_(2)) / T_(2) - (q_(2) - q_(0)) / T_(1) + + v0_ * T_(0) / T_(1) + a0_ * T_(0) * T_(0) / (3.0 * T_(1))); + + c(sys_size - 2) = + 6.0 * + ((q_(n_ - 1) - q_(n_ - 3)) / T_(n_segments - 2) - + (q_(n_ - 3) - q_(n_ - 4)) / T_(n_segments - 3) - + vn_ * T_(n_segments - 1) / T_(n_segments - 2) + + an_ * T_(n_segments - 1) * T_(n_segments - 1) / (3.0 * T_(n_segments - 2))); + + // Middle elements + for (int i = 2; i < sys_size - 2; ++i) { + if (i == sys_size - 3) { + continue; // Already handled second-to-last + } + c(i) = 6.0 * ((q_(i + 2) - q_(i + 1)) / T_(i + 1) - + (q_(i + 1) - q_(i)) / T_(i)); + } + } + + if (debug_) { + std::cout << "Main diagonal: " << main_diag.transpose() << "\n"; + std::cout << "Lower diagonal: " << lower_diag.transpose() << "\n"; + std::cout << "Upper diagonal: " << upper_diag.transpose() << "\n"; + std::cout << "Vector c: " << c.transpose() << "\n\n"; + } + + Eigen::VectorXd interior_omega = solve_tridiagonal(lower_diag, main_diag, upper_diag, c); + + // Complete accelerations vector + omega_.resize(n_); + omega_(0) = a0_; + omega_.segment(1, sys_size) = interior_omega; + omega_(n_ - 1) = an_; + + // Adjust extra point positions (equations 4.26, 4.27) + q_(1) = q_(0) + T_(0) * v0_ + (T_(0) * T_(0) / 3.0) * a0_ + + (T_(0) * T_(0) / 6.0) * omega_(1); + q_(n_ - 2) = q_(n_ - 1) - T_(n_segments - 1) * vn_ + + (T_(n_segments - 1) * T_(n_segments - 1) / 3.0) * an_ + + (T_(n_segments - 1) * T_(n_segments - 1) / 6.0) * omega_(n_ - 2); + + if (debug_) { + std::cout << "Computed accelerations: " << omega_.transpose() << "\n"; + std::cout << "Adjusted q1: " << q_(1) << "\n"; + std::cout << "Adjusted qn-1: " << q_(n_ - 2) << "\n\n"; + } +} + +void CubicSplineWithAcceleration1::compute_coefficients() { + int n_segments = n_ - 1; + coeffs_.resize(n_segments, 4); + + for (int k = 0; k < n_segments; ++k) { + coeffs_(k, 0) = q_(k); + coeffs_(k, 1) = + (q_(k + 1) - q_(k)) / T_(k) - (T_(k) / 6.0) * (omega_(k + 1) + 2.0 * omega_(k)); + coeffs_(k, 2) = omega_(k) / 2.0; + coeffs_(k, 3) = (omega_(k + 1) - omega_(k)) / (6.0 * T_(k)); + } + + if (debug_) { + std::cout << "Polynomial coefficients:\n"; + for (int k = 0; k < n_segments; ++k) { + std::cout << "Segment " << k << ": " << coeffs_.row(k) << "\n"; + } + } +} + +void CubicSplineWithAcceleration1::compute_original_indices() { + original_indices_.clear(); + original_indices_.push_back(0); + for (int i = 1; i < n_orig_ - 1; ++i) { + original_indices_.push_back(i + 1); + } + original_indices_.push_back(n_ - 1); +} + +CubicSplineWithAcceleration1::SegmentInfo +CubicSplineWithAcceleration1::find_segment(double t) const { + if (t <= t_(0)) { + return {0, 0.0}; + } + if (t >= t_(n_ - 1)) { + return {n_ - 2, T_(n_ - 2)}; + } + auto begin = t_.data(); + auto end = begin + n_; + auto it = std::upper_bound(begin, end, t); + int k = static_cast(it - begin) - 1; + return {k, t - t_(k)}; +} + +double CubicSplineWithAcceleration1::evaluate(double t) const { + auto [k, tau] = find_segment(t); + auto c = coeffs_.row(k); + return c(0) + c(1) * tau + c(2) * tau * tau + c(3) * tau * tau * tau; +} + +Eigen::VectorXd CubicSplineWithAcceleration1::evaluate(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate(t(i)); + } + return result; +} + +double CubicSplineWithAcceleration1::evaluate_velocity(double t) const { + auto [k, tau] = find_segment(t); + auto c = coeffs_.row(k); + return c(1) + 2.0 * c(2) * tau + 3.0 * c(3) * tau * tau; +} + +Eigen::VectorXd CubicSplineWithAcceleration1::evaluate_velocity(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_velocity(t(i)); + } + return result; +} + +double CubicSplineWithAcceleration1::evaluate_acceleration(double t) const { + auto [k, tau] = find_segment(t); + auto c = coeffs_.row(k); + return 2.0 * c(2) + 6.0 * c(3) * tau; +} + +Eigen::VectorXd +CubicSplineWithAcceleration1::evaluate_acceleration(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_acceleration(t(i)); + } + return result; +} + +} // namespace interpolatecpp::spline diff --git a/cpp/src/cubic_spline_with_acc2.cpp b/cpp/src/cubic_spline_with_acc2.cpp new file mode 100644 index 0000000..e079b6c --- /dev/null +++ b/cpp/src/cubic_spline_with_acc2.cpp @@ -0,0 +1,175 @@ +#include + +#include +#include + +namespace interpolatecpp::spline { + +CubicSplineWithAcceleration2::CubicSplineWithAcceleration2(std::span t_points, + std::span q_points, + const SplineParameters& params) + : CubicSpline(t_points, q_points, params.v0, params.vn, params.debug), + a0_(params.a0), + an_(params.an) { + if (params.a0.has_value()) { + replace_first_segment_with_quintic(); + } + if (params.an.has_value()) { + replace_last_segment_with_quintic(); + } +} + +void CubicSplineWithAcceleration2::replace_first_segment_with_quintic() { + double q0 = q_points_(0); + double q1 = q_points_(1); + double v0 = velocities_(0); + double v1 = velocities_(1); + double a0 = a0_.value(); + + // Acceleration at end of first segment from cubic coefficients + double a1 = 2.0 * coefficients_(0, 2) + 6.0 * coefficients_(0, 3) * t_intervals_(0); + + double T = t_intervals_(0); + + // 6x6 system: p(0)=q0, p'(0)=v0, p''(0)=a0, p(T)=q1, p'(T)=v1, p''(T)=a1 + Eigen::Matrix A; + A << 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 2, 0, 0, 0, + 1, T, T*T, T*T*T, T*T*T*T, T*T*T*T*T, + 0, 1, 2*T, 3*T*T, 4*T*T*T, 5*T*T*T*T, + 0, 0, 2, 6*T, 12*T*T, 20*T*T*T; + + Eigen::Vector b; + b << q0, v0, a0, q1, v1, a1; + + quintic_first_ = A.colPivHouseholderQr().solve(b); + + if (debug_) { + std::cout << "\nReplaced first segment with quintic polynomial:\n"; + for (int i = 0; i < 6; ++i) { + std::cout << " b" << i << " = " << quintic_first_.value()(i) << "\n"; + } + } +} + +void CubicSplineWithAcceleration2::replace_last_segment_with_quintic() { + double qn_1 = q_points_(n_); // last point + double qn_1_prev = q_points_(n_ - 1); // second-to-last + double vn = velocities_(n_); + double vn_1 = velocities_(n_ - 1); + double an = an_.value(); + + // Acceleration at start of last segment from cubic coefficients + double an_1 = 2.0 * coefficients_(n_ - 1, 2); + + double T = t_intervals_(n_ - 1); + + // 6x6 system + Eigen::Matrix A; + A << 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 2, 0, 0, 0, + 1, T, T*T, T*T*T, T*T*T*T, T*T*T*T*T, + 0, 1, 2*T, 3*T*T, 4*T*T*T, 5*T*T*T*T, + 0, 0, 2, 6*T, 12*T*T, 20*T*T*T; + + Eigen::Vector b; + b << qn_1_prev, vn_1, an_1, qn_1, vn, an; + + quintic_last_ = A.colPivHouseholderQr().solve(b); + + if (debug_) { + std::cout << "\nReplaced last segment with quintic polynomial:\n"; + for (int i = 0; i < 6; ++i) { + std::cout << " b" << i << " = " << quintic_last_.value()(i) << "\n"; + } + } +} + +double CubicSplineWithAcceleration2::evaluate(double t) const { + auto [k, tau] = find_segment(t); + + if (k == 0 && quintic_first_.has_value()) { + const auto& b = quintic_first_.value(); + return b(0) + b(1) * tau + b(2) * tau * tau + b(3) * tau * tau * tau + + b(4) * tau * tau * tau * tau + b(5) * tau * tau * tau * tau * tau; + } + if (k == n_ - 1 && quintic_last_.has_value()) { + const auto& b = quintic_last_.value(); + return b(0) + b(1) * tau + b(2) * tau * tau + b(3) * tau * tau * tau + + b(4) * tau * tau * tau * tau + b(5) * tau * tau * tau * tau * tau; + } + + double a0 = coefficients_(k, 0); + double a1 = coefficients_(k, 1); + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return a0 + a1 * tau + a2 * tau * tau + a3 * tau * tau * tau; +} + +Eigen::VectorXd CubicSplineWithAcceleration2::evaluate(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate(t(i)); + } + return result; +} + +double CubicSplineWithAcceleration2::evaluate_velocity(double t) const { + auto [k, tau] = find_segment(t); + + if (k == 0 && quintic_first_.has_value()) { + const auto& b = quintic_first_.value(); + return b(1) + 2.0 * b(2) * tau + 3.0 * b(3) * tau * tau + + 4.0 * b(4) * tau * tau * tau + 5.0 * b(5) * tau * tau * tau * tau; + } + if (k == n_ - 1 && quintic_last_.has_value()) { + const auto& b = quintic_last_.value(); + return b(1) + 2.0 * b(2) * tau + 3.0 * b(3) * tau * tau + + 4.0 * b(4) * tau * tau * tau + 5.0 * b(5) * tau * tau * tau * tau; + } + + double a1 = coefficients_(k, 1); + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return a1 + 2.0 * a2 * tau + 3.0 * a3 * tau * tau; +} + +Eigen::VectorXd CubicSplineWithAcceleration2::evaluate_velocity(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_velocity(t(i)); + } + return result; +} + +double CubicSplineWithAcceleration2::evaluate_acceleration(double t) const { + auto [k, tau] = find_segment(t); + + if (k == 0 && quintic_first_.has_value()) { + const auto& b = quintic_first_.value(); + return 2.0 * b(2) + 6.0 * b(3) * tau + 12.0 * b(4) * tau * tau + + 20.0 * b(5) * tau * tau * tau; + } + if (k == n_ - 1 && quintic_last_.has_value()) { + const auto& b = quintic_last_.value(); + return 2.0 * b(2) + 6.0 * b(3) * tau + 12.0 * b(4) * tau * tau + + 20.0 * b(5) * tau * tau * tau; + } + + double a2 = coefficients_(k, 2); + double a3 = coefficients_(k, 3); + return 2.0 * a2 + 6.0 * a3 * tau; +} + +Eigen::VectorXd +CubicSplineWithAcceleration2::evaluate_acceleration(const Eigen::VectorXd& t) const { + Eigen::VectorXd result(t.size()); + for (Eigen::Index i = 0; i < t.size(); ++i) { + result(i) = evaluate_acceleration(t(i)); + } + return result; +} + +} // namespace interpolatecpp::spline diff --git a/cpp/src/double_s_trajectory.cpp b/cpp/src/double_s_trajectory.cpp new file mode 100644 index 0000000..74688e6 --- /dev/null +++ b/cpp/src/double_s_trajectory.cpp @@ -0,0 +1,379 @@ +#include + +#include +#include + +namespace interpolatecpp::motion { + +DoubleSTrajectory::DoubleSTrajectory(const StateParams& state, const TrajectoryBounds& bounds) + : state_(state), + bounds_(bounds), + T_(0.0), + Ta_(0.0), + Tv_(0.0), + Td_(0.0), + Tj_1_(0.0), + Tj_2_(0.0), + a_lim_a_(0.0), + a_lim_d_(0.0), + v_lim_(0.0), + sigma_(1.0) { + if (std::abs(state_.v_0) > bounds_.v_bound || std::abs(state_.v_1) > bounds_.v_bound) { + throw std::invalid_argument( + "Initial or final velocities exceed the velocity bound of " + + std::to_string(bounds_.v_bound)); + } + + plan_trajectory(); +} + +void DoubleSTrajectory::plan_trajectory() { + const double qd_0 = state_.q_0; + const double qd_1 = state_.q_1; + const double vd_0 = state_.v_0; + const double vd_1 = state_.v_1; + + // BLOCK 1: Handle equal positions + if (std::abs(qd_1 - qd_0) < kEpsilon) { + if (std::abs(vd_1 - vd_0) < kEpsilon) { + // Same position, same velocity: static trajectory + T_ = 0.0; + return; + } + // Same position but different velocities: minimal trajectory + const double t_min = std::abs(vd_1 - vd_0) / bounds_.a_bound; + T_ = std::max(t_min * 1.5, 0.1); + return; + } + + // Normal case: different positions + sigma_ = (qd_1 > qd_0) ? 1.0 : -1.0; + + // Transform parameters based on direction + const double q_0_t = sigma_ * qd_0; + const double q_1_t = sigma_ * qd_1; + const double v_0_t = sigma_ * vd_0; + const double v_1_t = sigma_ * vd_1; + + // Set limits based on direction (sigma transform collapses to using positive bounds) + const double v_max = ((sigma_ + 1.0) / 2.0) * bounds_.v_bound + + ((sigma_ - 1.0) / 2.0) * (-bounds_.v_bound); + const double a_max_initial = ((sigma_ + 1.0) / 2.0) * bounds_.a_bound + + ((sigma_ - 1.0) / 2.0) * (-bounds_.a_bound); + const double j_max = ((sigma_ + 1.0) / 2.0) * bounds_.j_bound + + ((sigma_ - 1.0) / 2.0) * (-bounds_.j_bound); + + double a_max = a_max_initial; + + // Compute time intervals assuming v_max and a_max are reached + + // Acceleration part + if ((v_max - v_0_t) * j_max < a_max * a_max) { + Tj_1_ = std::sqrt(std::max((v_max - v_0_t) / j_max, 0.0)); + Ta_ = 2.0 * Tj_1_; + } else { + Tj_1_ = a_max / j_max; + Ta_ = Tj_1_ + (v_max - v_0_t) / a_max; + } + + // Deceleration part + if ((v_max - v_1_t) * j_max < a_max * a_max) { + Tj_2_ = std::sqrt(std::max((v_max - v_1_t) / j_max, 0.0)); + Td_ = 2.0 * Tj_2_; + } else { + Tj_2_ = a_max / j_max; + Td_ = Tj_2_ + (v_max - v_1_t) / a_max; + } + + // Determine the time duration of the constant velocity phase + if (std::abs(v_max) < kEpsilon) { + Tv_ = 0.0; + } else { + Tv_ = (q_1_t - q_0_t) / v_max - + (Ta_ / 2.0) * (1.0 + v_0_t / v_max) - + (Td_ / 2.0) * (1.0 + v_1_t / v_max); + } + + // Check if Tv < 0 (v_max is not reached): binary search on gamma + if (Tv_ < 0.0) { + Tv_ = 0.0; + + double gamma_high = 1.0; + double gamma_low = kMinGamma; + double gamma_mid = 0.5; + + for (int iteration = 0; iteration < kMaxIterations; ++iteration) { + gamma_mid = (gamma_high + gamma_low) / 2.0; + + const double a_max_test = gamma_mid * bounds_.a_bound; + + // Recalculate time intervals + const double tj = a_max_test / j_max; + const double delta = + (a_max_test * a_max_test * a_max_test * a_max_test) / (j_max * j_max) + + 2.0 * (v_0_t * v_0_t + v_1_t * v_1_t) + + a_max_test * ( + 4.0 * (q_1_t - q_0_t) - + 2.0 * a_max_test / j_max * (v_0_t + v_1_t) + ); + + // Check if delta is negative (no solution with current gamma) + if (delta < 0.0) { + gamma_high = gamma_mid; + continue; + } + + const double sqrt_delta = std::sqrt(delta); + + double ta = (a_max_test * a_max_test / j_max - 2.0 * v_0_t + sqrt_delta) / + (2.0 * a_max_test); + double td = (a_max_test * a_max_test / j_max - 2.0 * v_1_t + sqrt_delta) / + (2.0 * a_max_test); + + double tj_1_local = tj; + double tj_2_local = tj; + + if (ta < 0.0) { + if (std::abs(v_1_t + v_0_t) < kEpsilon) { + // Avoid division by zero + Ta_ = 0.0; + Td_ = 0.0; + Tj_1_ = 0.0; + Tj_2_ = 0.0; + break; + } + ta = 0.0; + td = 2.0 * (q_1_t - q_0_t) / (v_1_t + v_0_t); + const double inner = j_max * ( + j_max * (q_1_t - q_0_t) * (q_1_t - q_0_t) + + (v_1_t + v_0_t) * (v_1_t + v_0_t) * (v_1_t - v_0_t) + ); + const double tj_2_arg = + j_max * (q_1_t - q_0_t) - std::sqrt(inner); + tj_1_local = 0.0; + tj_2_local = (std::abs(tj_2_arg) > kEpsilon) + ? tj_2_arg / (j_max * (v_1_t + v_0_t)) + : 0.0; + + // Validate and store + if (tj_1_local >= 0.0 && tj_2_local >= 0.0 && ta >= 0.0 && td >= 0.0) { + a_max = a_max_test; + Tj_1_ = tj_1_local; + Tj_2_ = tj_2_local; + Ta_ = ta; + Td_ = td; + break; + } + gamma_high = gamma_mid; + } else if (td < 0.0) { + if (std::abs(v_1_t + v_0_t) < kEpsilon) { + // Avoid division by zero + Ta_ = 0.0; + Td_ = 0.0; + Tj_1_ = 0.0; + Tj_2_ = 0.0; + break; + } + td = 0.0; + ta = 2.0 * (q_1_t - q_0_t) / (v_1_t + v_0_t); + const double inner = j_max * ( + j_max * (q_1_t - q_0_t) * (q_1_t - q_0_t) - + (v_1_t + v_0_t) * (v_1_t + v_0_t) * (v_1_t - v_0_t) + ); + const double tj_1_arg = + j_max * (q_1_t - q_0_t) - std::sqrt(inner); + tj_1_local = (std::abs(tj_1_arg) > kEpsilon) + ? tj_1_arg / (j_max * (v_1_t + v_0_t)) + : 0.0; + tj_2_local = 0.0; + + // Validate and store + if (tj_1_local >= 0.0 && tj_2_local >= 0.0 && ta >= 0.0 && td >= 0.0) { + a_max = a_max_test; + Tj_1_ = tj_1_local; + Tj_2_ = tj_2_local; + Ta_ = ta; + Td_ = td; + break; + } + gamma_high = gamma_mid; + } else if (ta > 2.0 * tj && td > 2.0 * tj) { + // Valid solution found + a_max = a_max_test; + Tj_1_ = tj; + Tj_2_ = tj; + Ta_ = ta; + Td_ = td; + break; + } else { + // Need to reduce gamma further + gamma_high = gamma_mid; + } + } + } + + // Compute trajectory parameters + a_lim_a_ = j_max * Tj_1_; + a_lim_d_ = -j_max * Tj_2_; + + // Ensure non-negative time periods + Ta_ = std::max(Ta_, 0.0); + Td_ = std::max(Td_, 0.0); + Tv_ = std::max(Tv_, 0.0); + Tj_1_ = std::max(Tj_1_, 0.0); + Tj_2_ = std::max(Tj_2_, 0.0); + + // Calculate v_lim safely + if (Ta_ <= Tj_1_) { + v_lim_ = v_0_t + j_max * Ta_ * Ta_ / 2.0; + } else { + v_lim_ = v_0_t + (Ta_ - Tj_1_) * a_lim_a_; + } + + // Total trajectory time + T_ = Ta_ + Tv_ + Td_; + + // Round final time to discrete ticks (milliseconds) + T_ = std::round(T_ * 1000.0) / 1000.0; +} + +FullTrajectoryResult DoubleSTrajectory::evaluate(double t) const { + // Special case: equal positions with equal velocities + if (std::abs(state_.q_1 - state_.q_0) < kEpsilon && + std::abs(state_.v_1 - state_.v_0) < kEpsilon) { + return {state_.q_0, state_.v_0, 0.0, 0.0}; + } + + // Special case: equal positions with different velocities + if (std::abs(state_.q_1 - state_.q_0) < kEpsilon && + std::abs(state_.v_1 - state_.v_0) >= kEpsilon) { + const double t_norm = std::min(t / T_, 1.0); + const double qp_val = state_.v_0 + t_norm * (state_.v_1 - state_.v_0); + + const double phase = 2.0 * M_PI * t_norm; + const double amplitude = (state_.v_1 - state_.v_0) * T_ / (2.0 * M_PI); + const double q_val = state_.q_0 + amplitude * std::sin(phase); + + const double qpp_val = (state_.v_1 - state_.v_0) / T_ + + amplitude * (2.0 * M_PI / T_) * std::cos(phase); + const double qppp_val = -amplitude * (2.0 * M_PI / T_) * (2.0 * M_PI / T_) * + std::sin(phase); + + return {q_val, qp_val, qpp_val, qppp_val}; + } + + return evaluate_7phase(t); +} + +FullTrajectoryResult DoubleSTrajectory::evaluate_7phase(double t) const { + // Clamp t to [0, T] + t = std::clamp(t, 0.0, T_); + + // Handle zero or near-zero duration trajectory + if (T_ < kEpsilon) { + return {state_.q_1, state_.v_1, 0.0, 0.0}; + } + + // Use transformed coordinates + const double q_0 = sigma_ * state_.q_0; + const double q_1 = sigma_ * state_.q_1; + const double v_0_t = sigma_ * state_.v_0; + const double v_1_t = sigma_ * state_.v_1; + + // j_max and j_min in transformed space + const double j_max = ((sigma_ + 1.0) / 2.0) * bounds_.j_bound + + ((sigma_ - 1.0) / 2.0) * (-bounds_.j_bound); + const double j_min = -j_max; + + double q_val = 0.0; + double qp_val = 0.0; + double qpp_val = 0.0; + double qppp_val = 0.0; + + // Phase 1: t in [0, Tj_1] -- jerk ramp-up + if (t <= Tj_1_ && Tj_1_ > 0.0) { + q_val = q_0 + v_0_t * t + j_max * t * t * t / 6.0; + qp_val = v_0_t + j_max * t * t / 2.0; + qpp_val = j_max * t; + qppp_val = j_max; + } + // Phase 2: t in [Tj_1, Ta - Tj_1] -- constant acceleration + else if (t <= (Ta_ - Tj_1_) && Ta_ > Tj_1_) { + q_val = q_0 + v_0_t * t + + a_lim_a_ / 6.0 * (3.0 * t * t - 3.0 * Tj_1_ * t + Tj_1_ * Tj_1_); + qp_val = v_0_t + a_lim_a_ * (t - Tj_1_ / 2.0); + qpp_val = a_lim_a_; + qppp_val = 0.0; + } + // Phase 3: t in [Ta - Tj_1, Ta] -- jerk ramp-down (end of acceleration) + else if (t <= Ta_ && Ta_ > 0.0) { + const double dt = Ta_ - t; + q_val = q_0 + (v_lim_ + v_0_t) * Ta_ / 2.0 - + v_lim_ * dt - j_min * dt * dt * dt / 6.0; + qp_val = v_lim_ + j_min * dt * dt / 2.0; + qpp_val = -j_min * dt; + qppp_val = j_min; + } + // Phase 4: t in [Ta, Ta + Tv] -- constant velocity + else if (t <= (Ta_ + Tv_) && Tv_ > 0.0) { + q_val = q_0 + (v_lim_ + v_0_t) * Ta_ / 2.0 + v_lim_ * (t - Ta_); + qp_val = v_lim_; + qpp_val = 0.0; + qppp_val = 0.0; + } + // Phase 5: t in [Ta + Tv, Ta + Tv + Tj_2] -- start of deceleration + else if (t <= (Ta_ + Tv_ + Tj_2_) && Tj_2_ > 0.0) { + const double dt = t - T_ + Td_; + q_val = q_1 - (v_lim_ + v_1_t) * Td_ / 2.0 + + v_lim_ * dt - j_max * dt * dt * dt / 6.0; + qp_val = v_lim_ - j_max * dt * dt / 2.0; + qpp_val = -j_max * dt; + qppp_val = -j_max; + } + // Phase 6: t in [Ta + Tv + Tj_2, Ta + Tv + Td - Tj_2] -- constant deceleration + else if (t <= (Ta_ + Tv_ + Td_ - Tj_2_) && Td_ > Tj_2_) { + const double dt = t - T_ + Td_; + q_val = q_1 - (v_lim_ + v_1_t) * Td_ / 2.0 + + v_lim_ * dt + + a_lim_d_ / 6.0 * (3.0 * dt * dt - 3.0 * Tj_2_ * dt + Tj_2_ * Tj_2_); + qp_val = v_lim_ + a_lim_d_ * (dt - Tj_2_ / 2.0); + qpp_val = a_lim_d_; + qppp_val = 0.0; + } + // Phase 7: t in [T - Tj_2, T] -- final jerk ramp + else if (t <= T_ && Td_ > 0.0) { + const double dt = T_ - t; + q_val = q_1 - v_1_t * dt - j_max * dt * dt * dt / 6.0; + qp_val = v_1_t + j_max * dt * dt / 2.0; + qpp_val = -j_max * dt; + qppp_val = j_max; + } + // After end of trajectory or for empty phases + else { + q_val = q_1; + qp_val = v_1_t; + qpp_val = 0.0; + qppp_val = 0.0; + } + + // Transform back using sigma + return { + sigma_ * q_val, + sigma_ * qp_val, + sigma_ * qpp_val, + sigma_ * qppp_val + }; +} + +std::map DoubleSTrajectory::phase_durations() const { + return { + {"total", T_}, + {"acceleration", Ta_}, + {"constant_velocity", Tv_}, + {"deceleration", Td_}, + {"jerk_acceleration", Tj_1_}, + {"jerk_deceleration", Tj_2_} + }; +} + +} // namespace interpolatecpp::motion diff --git a/cpp/src/frenet_frame.cpp b/cpp/src/frenet_frame.cpp new file mode 100644 index 0000000..7b4ce06 --- /dev/null +++ b/cpp/src/frenet_frame.cpp @@ -0,0 +1,73 @@ +#include + +#include +#include + +namespace interpolatecpp::path { + +std::vector compute_frenet_frames( + const std::function( + double)>& curve, + const Eigen::VectorXd& s_values) { + std::vector frames; + frames.reserve(s_values.size()); + + for (Eigen::Index i = 0; i < s_values.size(); ++i) { + auto [pos, vel, acc] = curve(s_values[i]); + + FrenetFrame frame; + double vel_norm = vel.norm(); + + if (vel_norm < 1e-10) { + frame.tangent = Eigen::Vector3d::UnitX(); + frame.normal = Eigen::Vector3d::UnitY(); + frame.binormal = Eigen::Vector3d::UnitZ(); + frame.curvature = 0.0; + frame.torsion = 0.0; + } else { + frame.tangent = vel / vel_norm; + + Eigen::Vector3d cross = vel.cross(acc); + double cross_norm = cross.norm(); + + frame.curvature = cross_norm / (vel_norm * vel_norm * vel_norm); + + if (cross_norm < 1e-10) { + // Straight line - pick arbitrary normal + Eigen::Vector3d arbitrary = + (std::abs(frame.tangent.dot(Eigen::Vector3d::UnitX())) < 0.9) + ? Eigen::Vector3d::UnitX() + : Eigen::Vector3d::UnitY(); + frame.binormal = frame.tangent.cross(arbitrary).normalized(); + frame.normal = frame.binormal.cross(frame.tangent); + } else { + frame.binormal = cross / cross_norm; + frame.normal = frame.binormal.cross(frame.tangent); + } + + frame.torsion = 0.0; // Requires third derivative for torsion + } + + frames.push_back(frame); + } + + return frames; +} + +std::tuple +circular_trajectory_with_derivatives(double u, double r) { + Eigen::Vector3d p(r * std::cos(u), r * std::sin(u), 0.0); + Eigen::Vector3d dp(-r * std::sin(u), r * std::cos(u), 0.0); + Eigen::Vector3d d2p(-r * std::cos(u), -r * std::sin(u), 0.0); + return {p, dp, d2p}; +} + +std::tuple +helicoidal_trajectory_with_derivatives(double u, double r, double d) { + Eigen::Vector3d p(r * std::cos(u), r * std::sin(u), d * u); + Eigen::Vector3d dp(-r * std::sin(u), r * std::cos(u), d); + Eigen::Vector3d d2p(-r * std::cos(u), -r * std::sin(u), 0.0); + return {p, dp, d2p}; +} + +} // namespace interpolatecpp::path diff --git a/cpp/src/linear_path.cpp b/cpp/src/linear_path.cpp new file mode 100644 index 0000000..90d5e2c --- /dev/null +++ b/cpp/src/linear_path.cpp @@ -0,0 +1,36 @@ +#include + +#include + +namespace interpolatecpp::path { + +LinearPath::LinearPath(const Eigen::Vector3d& pi, const Eigen::Vector3d& pf) + : pi_(pi), pf_(pf) { + length_ = (pf_ - pi_).norm(); + if (length_ > 0) { + tangent_ = (pf_ - pi_) / length_; + } else { + tangent_ = Eigen::Vector3d::Zero(); + } +} + +Eigen::Vector3d LinearPath::position(double s) const { + s = std::clamp(s, 0.0, length_); + return (length_ > 0) ? pi_ + (s / length_) * (pf_ - pi_) : pi_; +} + +Eigen::MatrixXd LinearPath::position(const Eigen::VectorXd& s) const { + Eigen::MatrixXd result(s.size(), 3); + for (Eigen::Index i = 0; i < s.size(); ++i) { + result.row(i) = position(s[i]).transpose(); + } + return result; +} + +Eigen::Vector3d LinearPath::velocity(double /*s*/) const { return tangent_; } + +Eigen::Vector3d LinearPath::acceleration(double /*s*/) const { + return Eigen::Vector3d::Zero(); +} + +} // namespace interpolatecpp::path diff --git a/cpp/src/linear_traj.cpp b/cpp/src/linear_traj.cpp new file mode 100644 index 0000000..3bfe6c9 --- /dev/null +++ b/cpp/src/linear_traj.cpp @@ -0,0 +1,31 @@ +#include + +namespace interpolatecpp::path { + +LinearTrajResult linear_traj(const Eigen::VectorXd& p0, const Eigen::VectorXd& p1, + double t0, double t1, int num_points) { + const int dim = static_cast(p0.size()); + const double dt = (num_points > 1) ? (t1 - t0) / (num_points - 1) : 0.0; + Eigen::VectorXd vel; + if (t1 > t0) { + vel = (p1 - p0) / (t1 - t0); + } else { + vel = Eigen::VectorXd::Zero(dim); + } + + LinearTrajResult result; + result.positions = Eigen::MatrixXd(num_points, dim); + result.velocities = Eigen::MatrixXd(num_points, dim); + result.accelerations = Eigen::MatrixXd::Zero(num_points, dim); + + for (int i = 0; i < num_points; ++i) { + double t = t0 + i * dt; + double u = (t1 > t0) ? (t - t0) / (t1 - t0) : 0.0; + result.positions.row(i) = (p0 + u * (p1 - p0)).transpose(); + result.velocities.row(i) = vel.transpose(); + } + + return result; +} + +} // namespace interpolatecpp::path diff --git a/cpp/src/log_quaternion_interpolation.cpp b/cpp/src/log_quaternion_interpolation.cpp new file mode 100644 index 0000000..5af5661 --- /dev/null +++ b/cpp/src/log_quaternion_interpolation.cpp @@ -0,0 +1,132 @@ +#include + +#include +#include + +namespace interpolatecpp::quat { + +Eigen::MatrixXd LogQuaternionInterpolation::recover_continuous_axis_angle() const { + const int n = static_cast(quaternions_.size()); + Eigen::MatrixXd aa_vectors(n, 3); + + // Working copy for double-cover handling + std::vector quats = quaternions_; + + // Ensure double-cover continuity + for (int i = 1; i < n; ++i) { + double dot_pos = quats[i - 1].dot_product(quats[i]); + double dot_neg = quats[i - 1].dot_product(-quats[i]); + if (dot_neg > dot_pos) { + quats[i] = -quats[i]; + } + } + + // Extract axis-angle and handle continuity + std::vector angles(n); + std::vector axes(n); + + for (int i = 0; i < n; ++i) { + auto [axis, angle] = quats[i].to_axis_angle(); + angles[i] = angle; + axes[i] = axis; + } + + // Axis continuity: flip axis if closer to -prev_axis + for (int i = 1; i < n; ++i) { + if ((axes[i - 1] - axes[i]).norm() > (axes[i - 1] + axes[i]).norm()) { + angles[i] = -angles[i]; + axes[i] = -axes[i]; + } + } + + // Phase unwrap angles + for (int i = 1; i < n; ++i) { + double diff = angles[i] - angles[i - 1]; + while (diff > M_PI) { + angles[i] -= 2.0 * M_PI; + diff -= 2.0 * M_PI; + } + while (diff < -M_PI) { + angles[i] += 2.0 * M_PI; + diff += 2.0 * M_PI; + } + } + + // Convert to axis-angle vectors: r = theta * axis + for (int i = 0; i < n; ++i) { + aa_vectors.row(i) = angles[i] * axes[i].transpose(); + } + + return aa_vectors; +} + +LogQuaternionInterpolation::LogQuaternionInterpolation( + const std::vector& time_points, const std::vector& quaternions, + int degree, const std::optional& initial_velocity, + const std::optional& final_velocity) + : times_(time_points), quaternions_(quaternions) { + if (time_points.size() != quaternions.size()) { + throw std::invalid_argument("Time points and quaternions must have same length"); + } + if (quaternions.size() < 2) { + throw std::invalid_argument("Need at least 2 quaternions"); + } + if (degree < 3 || degree > 5) { + throw std::invalid_argument("Degree must be 3, 4, or 5"); + } + if (static_cast(quaternions.size()) < degree + 1) { + throw std::invalid_argument("Need at least degree+1 quaternions"); + } + for (size_t i = 1; i < time_points.size(); ++i) { + if (time_points[i] <= time_points[i - 1]) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + // Normalize quaternions + for (auto& q : quaternions_) { + q = q.unit(); + } + + // Recover continuous axis-angle representation + Eigen::MatrixXd aa_vectors = recover_continuous_axis_angle(); + + // Create time vector for B-spline + Eigen::VectorXd times_eigen(times_.size()); + for (size_t i = 0; i < times_.size(); ++i) { + times_eigen[i] = times_[i]; + } + + // Create B-spline interpolator in axis-angle space + spline_ = std::make_unique( + degree, aa_vectors, times_eigen, initial_velocity, final_velocity); +} + +Quaternion LogQuaternionInterpolation::evaluate(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + + Eigen::VectorXd r = spline_->evaluate(t); + + // Convert axis-angle vector back to quaternion + double theta = r.norm(); + if (theta < kEpsilon) { + return Quaternion::identity(); + } + + Eigen::Vector3d axis = r / theta; + return Quaternion::from_angle_axis(theta, axis); +} + +Eigen::Vector3d LogQuaternionInterpolation::evaluate_velocity(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + Eigen::VectorXd dr = spline_->evaluate_derivative(t, 1); + return Eigen::Vector3d(dr[0], dr[1], dr[2]); +} + +Eigen::Vector3d LogQuaternionInterpolation::evaluate_acceleration(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + Eigen::VectorXd ddr = spline_->evaluate_derivative(t, 2); + return Eigen::Vector3d(ddr[0], ddr[1], ddr[2]); +} + +} // namespace interpolatecpp::quat diff --git a/cpp/src/modified_log_quaternion_interpolation.cpp b/cpp/src/modified_log_quaternion_interpolation.cpp new file mode 100644 index 0000000..66e80f4 --- /dev/null +++ b/cpp/src/modified_log_quaternion_interpolation.cpp @@ -0,0 +1,163 @@ +#include + +#include +#include + +namespace interpolatecpp::quat { + +void ModifiedLogQuaternionInterpolation::ensure_quaternion_continuity() { + for (size_t i = 1; i < quaternions_.size(); ++i) { + double dot_pos = quaternions_[i - 1].dot_product(quaternions_[i]); + double dot_neg = quaternions_[i - 1].dot_product(-quaternions_[i]); + if (dot_neg > dot_pos) { + quaternions_[i] = -quaternions_[i]; + } + } +} + +std::pair +ModifiedLogQuaternionInterpolation::transform_to_theta_xyz() const { + const int n = static_cast(quaternions_.size()); + Eigen::VectorXd theta_values(n); + Eigen::MatrixXd xyz_values(n, 3); + + for (int i = 0; i < n; ++i) { + auto [axis, angle] = quaternions_[i].to_axis_angle(); + theta_values[i] = angle; + xyz_values.row(i) = axis.transpose(); + } + + return {theta_values, xyz_values}; +} + +ModifiedLogQuaternionInterpolation::ModifiedLogQuaternionInterpolation( + const std::vector& time_points, const std::vector& quaternions, + int degree, bool normalize_axis, + const std::optional& initial_velocity, + const std::optional& final_velocity) + : times_(time_points), quaternions_(quaternions), normalize_axis_(normalize_axis) { + if (time_points.size() != quaternions.size()) { + throw std::invalid_argument("Time points and quaternions must have same length"); + } + if (quaternions.size() < 2) { + throw std::invalid_argument("Need at least 2 quaternions"); + } + if (degree < 3 || degree > 5) { + throw std::invalid_argument("Degree must be 3, 4, or 5"); + } + if (static_cast(quaternions.size()) < degree + 1) { + throw std::invalid_argument("Need at least degree+1 quaternions"); + } + for (size_t i = 1; i < time_points.size(); ++i) { + if (time_points[i] <= time_points[i - 1]) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + // Normalize quaternions + for (auto& q : quaternions_) { + q = q.unit(); + } + + // Handle double-cover for continuity + ensure_quaternion_continuity(); + + // Transform to (theta, X, Y, Z) representation + auto [theta_values, xyz_values] = transform_to_theta_xyz(); + + // Create time vector + Eigen::VectorXd times_eigen(times_.size()); + for (size_t i = 0; i < times_.size(); ++i) { + times_eigen[static_cast(i)] = times_[i]; + } + + // Split velocity constraints if provided + std::optional theta_init_vel = std::nullopt; + std::optional xyz_init_vel = std::nullopt; + std::optional theta_final_vel = std::nullopt; + std::optional xyz_final_vel = std::nullopt; + + if (initial_velocity.has_value()) { + const auto& iv = initial_velocity.value(); + if (iv.size() != 4) { + throw std::invalid_argument("initial_velocity must have exactly 4 elements"); + } + theta_init_vel = Eigen::VectorXd::Constant(1, iv[0]); + xyz_init_vel = iv.segment(1, 3); + } + + if (final_velocity.has_value()) { + const auto& fv = final_velocity.value(); + if (fv.size() != 4) { + throw std::invalid_argument("final_velocity must have exactly 4 elements"); + } + theta_final_vel = Eigen::VectorXd::Constant(1, fv[0]); + xyz_final_vel = fv.segment(1, 3); + } + + // Create separate B-spline interpolators for theta (1D) and XYZ (3D) + Eigen::MatrixXd theta_matrix = theta_values.reshaped(theta_values.size(), 1); + + theta_spline_ = std::make_unique( + degree, theta_matrix, times_eigen, theta_init_vel, theta_final_vel); + + xyz_spline_ = std::make_unique( + degree, xyz_values, times_eigen, xyz_init_vel, xyz_final_vel); +} + +Quaternion ModifiedLogQuaternionInterpolation::evaluate(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + + // Boundary cases + if (std::abs(t - times_.front()) <= kEpsilon) { + return quaternions_.front(); + } + if (std::abs(t - times_.back()) <= kEpsilon) { + return quaternions_.back(); + } + + // Evaluate both interpolators + double theta = theta_spline_->evaluate(t)[0]; + Eigen::VectorXd xyz_vec = xyz_spline_->evaluate(t); + Eigen::Vector3d xyz(xyz_vec[0], xyz_vec[1], xyz_vec[2]); + + // Optionally normalize the axis + if (normalize_axis_) { + double norm_xyz = xyz.norm(); + if (norm_xyz > kEpsilon) { + xyz /= norm_xyz; + } else { + xyz = Eigen::Vector3d::UnitX(); + } + } + + // Convert back to quaternion: q = [cos(theta/2), sin(theta/2) * axis] + if (std::abs(theta) < kEpsilon) { + return Quaternion::identity(); + } + + double cos_half = std::cos(theta / 2.0); + double sin_half = std::sin(theta / 2.0); + + return Quaternion(cos_half, sin_half * xyz[0], sin_half * xyz[1], sin_half * xyz[2]); +} + +Eigen::Vector4d ModifiedLogQuaternionInterpolation::evaluate_velocity(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + + double theta_dot = theta_spline_->evaluate_derivative(t, 1)[0]; + Eigen::VectorXd xyz_dot = xyz_spline_->evaluate_derivative(t, 1); + + return Eigen::Vector4d(theta_dot, xyz_dot[0], xyz_dot[1], xyz_dot[2]); +} + +Eigen::Vector4d ModifiedLogQuaternionInterpolation::evaluate_acceleration(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + + double theta_ddot = theta_spline_->evaluate_derivative(t, 2)[0]; + Eigen::VectorXd xyz_ddot = xyz_spline_->evaluate_derivative(t, 2); + + return Eigen::Vector4d(theta_ddot, xyz_ddot[0], xyz_ddot[1], xyz_ddot[2]); +} + +} // namespace interpolatecpp::quat diff --git a/cpp/src/parabolic_blend_trajectory.cpp b/cpp/src/parabolic_blend_trajectory.cpp new file mode 100644 index 0000000..14f4a12 --- /dev/null +++ b/cpp/src/parabolic_blend_trajectory.cpp @@ -0,0 +1,189 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::motion { + +void ParabolicBlendTrajectory::build_regions() { + if (n_ <= 0) { + n_regions_ = 0; + return; + } + + if (n_ == 1) { + // Single waypoint: one blend region + n_regions_ = 1; + reg_t0_.resize(1); + reg_t1_.resize(1); + reg_q0_.resize(1); + reg_v0_.resize(1); + reg_a_.resize(1); + + reg_t0_[0] = t_[0] - dt_blend_[0] / 2.0; + reg_t1_[0] = t_[0] + dt_blend_[0] / 2.0; + reg_q0_[0] = q_[0]; + reg_v0_[0] = 0.0; + reg_a_[0] = 0.0; + return; + } + + // Compute segment velocities + Eigen::VectorXd v_before = Eigen::VectorXd::Zero(n_); + Eigen::VectorXd v_after = Eigen::VectorXd::Zero(n_); + + for (int k = 0; k < n_ - 1; ++k) { + double dt = t_[k + 1] - t_[k]; + if (std::abs(dt) > 1e-15) { + v_before[k + 1] = (q_[k + 1] - q_[k]) / dt; + } + } + if (n_ >= 2) { + double dt0 = t_[1] - t_[0]; + if (std::abs(dt0) > 1e-15) { + v_before[0] = 0.0; // Zero at start + } + } + + // v_after[k] = v_before[k+1] for k < N-1, v_after[N-1] = 0 + for (int k = 0; k < n_ - 1; ++k) { + v_after[k] = v_before[k + 1]; + } + v_after[n_ - 1] = 0.0; + + // Also set v_before for segment velocities (as linear slopes) + Eigen::VectorXd seg_v = Eigen::VectorXd::Zero(n_ - 1); + for (int k = 0; k < n_ - 1; ++k) { + double dt = t_[k + 1] - t_[k]; + if (std::abs(dt) > 1e-15) { + seg_v[k] = (q_[k + 1] - q_[k]) / dt; + } + } + + // Compute accelerations at each waypoint + Eigen::VectorXd acc = Eigen::VectorXd::Zero(n_); + for (int k = 0; k < n_; ++k) { + double vb = (k > 0) ? seg_v[k - 1] : 0.0; + double va = (k < n_ - 1) ? seg_v[k] : 0.0; + double db = dt_blend_[k]; + if (std::abs(db) > 1e-15) { + acc[k] = (va - vb) / db; + } + } + + // Build 2N-1 regions + n_regions_ = 2 * n_ - 1; + reg_t0_.resize(n_regions_); + reg_t1_.resize(n_regions_); + reg_q0_.resize(n_regions_); + reg_v0_.resize(n_regions_); + reg_a_.resize(n_regions_); + + int r = 0; + + // Initial blend + double blend_start = t_[0] - dt_blend_[0] / 2.0; + double blend_end = t_[0] + dt_blend_[0] / 2.0; + reg_t0_[r] = blend_start; + reg_t1_[r] = blend_end; + reg_q0_[r] = q_[0] - 0.0 * (dt_blend_[0] / 2.0) - + 0.5 * acc[0] * (dt_blend_[0] / 2.0) * (dt_blend_[0] / 2.0); + reg_v0_[r] = 0.0 - acc[0] * (dt_blend_[0] / 2.0); + // Actually: at blend start, position from quadratic backwards + // q at blend center = q_[0], v at blend center = (vb+va)/2 + double vb_0 = 0.0; + reg_v0_[r] = vb_0; + reg_q0_[r] = q_[0] - vb_0 * (dt_blend_[0] / 2.0) - + 0.5 * acc[0] * (dt_blend_[0] / 2.0) * (dt_blend_[0] / 2.0); + reg_a_[r] = acc[0]; + ++r; + + // For each segment k (0 to N-2) + for (int k = 0; k < n_ - 1; ++k) { + // Constant-velocity region + double cv_start = t_[k] + dt_blend_[k] / 2.0; + double cv_end = t_[k + 1] - dt_blend_[k + 1] / 2.0; + + // Position at start of CV = position at end of previous blend + double u_prev = reg_t1_[r - 1] - reg_t0_[r - 1]; + double q_cv_start = + reg_q0_[r - 1] + reg_v0_[r - 1] * u_prev + 0.5 * reg_a_[r - 1] * u_prev * u_prev; + + reg_t0_[r] = cv_start; + reg_t1_[r] = cv_end; + reg_q0_[r] = q_cv_start; + reg_v0_[r] = seg_v[k]; + reg_a_[r] = 0.0; + ++r; + + // Blend at waypoint k+1 + double blend_s = t_[k + 1] - dt_blend_[k + 1] / 2.0; + double blend_e = t_[k + 1] + dt_blend_[k + 1] / 2.0; + + // Position at start of blend = position at end of CV + double u_cv = reg_t1_[r - 1] - reg_t0_[r - 1]; + double q_blend_start = reg_q0_[r - 1] + reg_v0_[r - 1] * u_cv; + + reg_t0_[r] = blend_s; + reg_t1_[r] = blend_e; + reg_q0_[r] = q_blend_start; + reg_v0_[r] = seg_v[k]; + reg_a_[r] = acc[k + 1]; + ++r; + } +} + +int ParabolicBlendTrajectory::find_region(double t_abs) const { + // Binary search on region start times + int lo = 0, hi = n_regions_ - 1; + while (lo < hi) { + int mid = (lo + hi + 1) / 2; + if (t_abs >= reg_t0_[mid]) { + lo = mid; + } else { + hi = mid - 1; + } + } + return lo; +} + +ParabolicBlendTrajectory::ParabolicBlendTrajectory(const std::vector& q, + const std::vector& t, + const std::vector& dt_blend) + : n_(static_cast(q.size())) { + if (q.size() != t.size() || q.size() != dt_blend.size()) { + throw std::invalid_argument("q, t, and dt_blend must have the same length"); + } + + q_ = Eigen::Map(q.data(), n_); + t_ = Eigen::Map(t.data(), n_); + dt_blend_ = Eigen::Map(dt_blend.data(), n_); + + build_regions(); +} + +TrajectoryResult ParabolicBlendTrajectory::evaluate(double t) const { + if (n_regions_ == 0) return {0.0, 0.0, 0.0}; + + // Clamp to valid range + double t_min = reg_t0_[0]; + double t_max = reg_t1_[n_regions_ - 1]; + t = std::clamp(t, t_min, t_max); + + int r = find_region(t); + double u = t - reg_t0_[r]; + + double pos = reg_q0_[r] + reg_v0_[r] * u + 0.5 * reg_a_[r] * u * u; + double vel = reg_v0_[r] + reg_a_[r] * u; + double acc = reg_a_[r]; + + return {pos, vel, acc}; +} + +double ParabolicBlendTrajectory::duration() const { + if (n_ < 2) return 0.0; + return t_[n_ - 1] - t_[0] + (dt_blend_[0] + dt_blend_[n_ - 1]) / 2.0; +} + +} // namespace interpolatecpp::motion diff --git a/cpp/src/polynomial_trajectory.cpp b/cpp/src/polynomial_trajectory.cpp new file mode 100644 index 0000000..99670bf --- /dev/null +++ b/cpp/src/polynomial_trajectory.cpp @@ -0,0 +1,227 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::motion { + +void PolynomialTrajectory::compute_order3(const BoundaryCondition& start, + const BoundaryCondition& end, double /*h*/) { + double T = t_end_ - t_start_; + double disp = end.position - start.position; + + coeffs_.resize(4); + coeffs_[0] = start.position; + coeffs_[1] = start.velocity; + coeffs_[2] = (3.0 * disp - (2.0 * start.velocity + end.velocity) * T) / (T * T); + coeffs_[3] = (-2.0 * disp + (start.velocity + end.velocity) * T) / (T * T * T); +} + +void PolynomialTrajectory::compute_order5(const BoundaryCondition& start, + const BoundaryCondition& end, double /*h*/) { + double T = t_end_ - t_start_; + double disp = end.position - start.position; + double T2 = T * T; + double T3 = T2 * T; + double T4 = T3 * T; + double T5 = T4 * T; + + coeffs_.resize(6); + coeffs_[0] = start.position; + coeffs_[1] = start.velocity; + coeffs_[2] = start.acceleration / 2.0; + coeffs_[3] = (1.0 / (2.0 * T3)) * + (20.0 * disp - (8.0 * end.velocity + 12.0 * start.velocity) * T - + (3.0 * start.acceleration - end.acceleration) * T2); + coeffs_[4] = (1.0 / (2.0 * T4)) * + (-30.0 * disp + (14.0 * end.velocity + 16.0 * start.velocity) * T + + (3.0 * start.acceleration - 2.0 * end.acceleration) * T2); + coeffs_[5] = (1.0 / (2.0 * T5)) * + (12.0 * disp - 6.0 * (end.velocity + start.velocity) * T + + (end.acceleration - start.acceleration) * T2); +} + +void PolynomialTrajectory::compute_order7(const BoundaryCondition& start, + const BoundaryCondition& end, double /*h*/) { + double T = t_end_ - t_start_; + double disp = end.position - start.position; + double T2 = T * T; + double T4 = T2 * T2; + double T5 = T4 * T; + double T6 = T5 * T; + double T7 = T6 * T; + + coeffs_.resize(8); + coeffs_[0] = start.position; + coeffs_[1] = start.velocity; + coeffs_[2] = start.acceleration / 2.0; + coeffs_[3] = start.jerk / 6.0; + coeffs_[4] = + (210.0 * disp - + T * ((30.0 * start.acceleration - 15.0 * end.acceleration) * T + + (4.0 * start.jerk + end.jerk) * T2 + 120.0 * start.velocity + + 90.0 * end.velocity)) / + (6.0 * T4); + coeffs_[5] = + (-168.0 * disp + + T * ((20.0 * start.acceleration - 14.0 * end.acceleration) * T + + (2.0 * start.jerk + end.jerk) * T2 + 90.0 * start.velocity + + 78.0 * end.velocity)) / + (2.0 * T5); + coeffs_[6] = + (420.0 * disp - + T * ((45.0 * start.acceleration - 39.0 * end.acceleration) * T + + (4.0 * start.jerk + 3.0 * end.jerk) * T2 + 216.0 * start.velocity + + 204.0 * end.velocity)) / + (6.0 * T6); + coeffs_[7] = + (-120.0 * disp + + T * ((12.0 * start.acceleration - 12.0 * end.acceleration) * T + + (start.jerk + end.jerk) * T2 + 60.0 * start.velocity + + 60.0 * end.velocity)) / + (6.0 * T7); +} + +PolynomialTrajectory::PolynomialTrajectory(const BoundaryCondition& bc_start, + const BoundaryCondition& bc_end, + const TimeInterval& interval, int order) + : order_(order), t_start_(interval.start), t_end_(interval.end) { + double h = interval.duration(); + switch (order) { + case ORDER_3: + compute_order3(bc_start, bc_end, h); + break; + case ORDER_5: + compute_order5(bc_start, bc_end, h); + break; + case ORDER_7: + compute_order7(bc_start, bc_end, h); + break; + default: + throw std::invalid_argument("Order must be 3, 5, or 7"); + } +} + +FullTrajectoryResult PolynomialTrajectory::evaluate(double t) const { + t = std::clamp(t, t_start_, t_end_); + double tau = t - t_start_; + + double q = 0.0, qd = 0.0, qdd = 0.0, qddd = 0.0; + const int n = static_cast(coeffs_.size()); + + // Horner-like evaluation for each derivative + if (n >= 4) { + // Position + q = coeffs_[0] + tau * (coeffs_[1] + tau * (coeffs_[2] + tau * coeffs_[3])); + qd = coeffs_[1] + tau * (2.0 * coeffs_[2] + tau * 3.0 * coeffs_[3]); + qdd = 2.0 * coeffs_[2] + 6.0 * coeffs_[3] * tau; + qddd = 6.0 * coeffs_[3]; + } + if (n >= 6) { + double tau2 = tau * tau; + double tau3 = tau2 * tau; + double tau4 = tau3 * tau; + q += coeffs_[4] * tau4 + coeffs_[5] * tau4 * tau; + qd += 4.0 * coeffs_[4] * tau3 + 5.0 * coeffs_[5] * tau4; + qdd += 12.0 * coeffs_[4] * tau2 + 20.0 * coeffs_[5] * tau3; + qddd += 24.0 * coeffs_[4] * tau + 60.0 * coeffs_[5] * tau2; + } + if (n >= 8) { + double tau2 = tau * tau; + double tau3 = tau2 * tau; + double tau4 = tau3 * tau; + double tau5 = tau4 * tau; + double tau6 = tau5 * tau; + q += coeffs_[6] * tau5 * tau + coeffs_[7] * tau6 * tau; + qd += 6.0 * coeffs_[6] * tau5 + 7.0 * coeffs_[7] * tau6; + qdd += 30.0 * coeffs_[6] * tau4 + 42.0 * coeffs_[7] * tau5; + qddd += 120.0 * coeffs_[6] * tau3 + 210.0 * coeffs_[7] * tau4; + } + + return {q, qd, qdd, qddd}; +} + +std::vector PolynomialTrajectory::heuristic_velocities( + const std::vector& points, const std::vector& times) { + const int n = static_cast(points.size()); + if (n < 2) { + throw std::invalid_argument("Need at least 2 points for velocity computation"); + } + + std::vector velocities(n, 0.0); + + // Compute slopes + std::vector slopes(n - 1); + for (int i = 0; i < n - 1; ++i) { + double dt = times[i + 1] - times[i]; + slopes[i] = (dt > 1e-10) ? (points[i + 1] - points[i]) / dt : 0.0; + } + + // Interior velocities: average of adjacent slopes, zero at sign changes + for (int i = 1; i < n - 1; ++i) { + if (slopes[i - 1] * slopes[i] > 0) { + velocities[i] = (slopes[i - 1] + slopes[i]) / 2.0; + } else { + velocities[i] = 0.0; + } + } + + return velocities; +} + +std::vector PolynomialTrajectory::multipoint_trajectory( + const std::vector& points, const std::vector& times, int order, + double v0, double vn) { + const int n = static_cast(points.size()); + if (n < 2) { + throw std::invalid_argument("Need at least 2 points"); + } + + auto velocities = heuristic_velocities(points, times); + velocities[0] = v0; + velocities[n - 1] = vn; + + std::vector segments; + segments.reserve(n - 1); + + for (int i = 0; i < n - 1; ++i) { + BoundaryCondition bc_start{points[i], velocities[i], 0.0, 0.0}; + BoundaryCondition bc_end{points[i + 1], velocities[i + 1], 0.0, 0.0}; + TimeInterval interval{times[i], times[i + 1]}; + segments.emplace_back(bc_start, bc_end, interval, order); + } + + return segments; +} + +FullTrajectoryResult PolynomialTrajectory::evaluate_multipoint( + const std::vector& segments, double t) { + if (segments.empty()) { + return {0.0, 0.0, 0.0, 0.0}; + } + + // Clamp to valid range + if (t <= segments.front().t_start()) { + return segments.front().evaluate(segments.front().t_start()); + } + if (t >= segments.back().t_end()) { + return segments.back().evaluate(segments.back().t_end()); + } + + // Binary search for segment + int lo = 0; + int hi = static_cast(segments.size()) - 1; + while (lo < hi) { + int mid = (lo + hi) / 2; + if (t >= segments[mid].t_end()) { + lo = mid + 1; + } else { + hi = mid; + } + } + + return segments[lo].evaluate(t); +} + +} // namespace interpolatecpp::motion diff --git a/cpp/src/quaternion.cpp b/cpp/src/quaternion.cpp new file mode 100644 index 0000000..fa62abf --- /dev/null +++ b/cpp/src/quaternion.cpp @@ -0,0 +1,252 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::quat { + +Quaternion::Quaternion(double w, double x, double y, double z) + : q_(w, x, y, z) {} + +Quaternion::Quaternion(const Eigen::Quaterniond& eq) : q_(eq) {} + +Quaternion Quaternion::identity() { return Quaternion(1.0, 0.0, 0.0, 0.0); } + +Quaternion Quaternion::from_angle_axis(double angle, const Eigen::Vector3d& axis) { + Eigen::Vector3d n = axis.normalized(); + double half = angle / 2.0; + return Quaternion(std::cos(half), n.x() * std::sin(half), n.y() * std::sin(half), + n.z() * std::sin(half)); +} + +Quaternion Quaternion::from_euler_angles(double roll, double pitch, double yaw) { + Eigen::Quaterniond eq = + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); + return Quaternion(eq); +} + +Quaternion Quaternion::operator*(const Quaternion& other) const { + return Quaternion(q_ * other.q_); +} + +Quaternion Quaternion::operator*(double scalar) const { + return Quaternion(q_.w() * scalar, q_.x() * scalar, q_.y() * scalar, + q_.z() * scalar); +} + +Quaternion Quaternion::operator+(const Quaternion& other) const { + return Quaternion(q_.w() + other.q_.w(), q_.x() + other.q_.x(), + q_.y() + other.q_.y(), q_.z() + other.q_.z()); +} + +Quaternion Quaternion::operator-(const Quaternion& other) const { + return Quaternion(q_.w() - other.q_.w(), q_.x() - other.q_.x(), + q_.y() - other.q_.y(), q_.z() - other.q_.z()); +} + +Quaternion Quaternion::operator-() const { + return Quaternion(-q_.w(), -q_.x(), -q_.y(), -q_.z()); +} + +Quaternion Quaternion::conjugate() const { + return Quaternion(q_.w(), -q_.x(), -q_.y(), -q_.z()); +} + +Quaternion Quaternion::inverse() const { + double ns = norm_squared(); + if (ns < kEpsilon * kEpsilon) { + return identity(); + } + auto c = conjugate(); + return Quaternion(c.w() / ns, c.x() / ns, c.y() / ns, c.z() / ns); +} + +Quaternion Quaternion::unit() const { + double n = norm(); + if (n < kEpsilon) { + return identity(); + } + return Quaternion(q_.w() / n, q_.x() / n, q_.y() / n, q_.z() / n); +} + +double Quaternion::norm() const noexcept { return q_.norm(); } + +double Quaternion::norm_squared() const noexcept { return q_.squaredNorm(); } + +double Quaternion::dot_product(const Quaternion& other) const { + return q_.w() * other.q_.w() + q_.x() * other.q_.x() + q_.y() * other.q_.y() + + q_.z() * other.q_.z(); +} + +Quaternion Quaternion::exp(const Quaternion& q) { + Eigen::Vector3d v(q.x(), q.y(), q.z()); + double theta = v.norm(); + + if (theta < kEpsilon) { + return Quaternion(1.0, v.x(), v.y(), v.z()); + } + + double sinc = std::sin(theta) / theta; + return Quaternion(std::cos(theta), v.x() * sinc, v.y() * sinc, v.z() * sinc); +} + +Quaternion Quaternion::log(const Quaternion& q) { + double s = std::clamp(q.w(), -1.0, 1.0); + double theta = std::acos(s); + Eigen::Vector3d v = q.vec(); + double sin_theta = std::sin(theta); + + if (std::abs(sin_theta) < kEpsilon) { + return Quaternion(0.0, v.x(), v.y(), v.z()); + } + + double factor = theta / sin_theta; + return Quaternion(0.0, v.x() * factor, v.y() * factor, v.z() * factor); +} + +Quaternion Quaternion::power(const Quaternion& q, double t) { + return exp(log(q) * t); +} + +Quaternion Quaternion::slerp(const Quaternion& q0, const Quaternion& q1, double t) { + // Handle double-cover + Quaternion target = q1; + if (q0.dot_product(q1) < 0.0) { + target = -q1; + } + + Quaternion rel = q0.inverse() * target; + return q0 * power(rel, t); +} + +Quaternion Quaternion::slerp_prime(const Quaternion& q0, const Quaternion& q1, + double t) { + Quaternion target = q1; + if (q0.dot_product(q1) < 0.0) { + target = -q1; + } + + Quaternion rel = q0.inverse() * target; + Quaternion log_rel = log(rel); + Quaternion result = slerp(q0, target, t); + return result * log_rel; +} + +Quaternion Quaternion::squad(const Quaternion& p, const Quaternion& a, + const Quaternion& b, const Quaternion& q, double t) { + Quaternion slerp_pq = slerp(p, q, t); + Quaternion slerp_ab = slerp(a, b, t); + return slerp(slerp_pq, slerp_ab, 2.0 * t * (1.0 - t)); +} + +Quaternion Quaternion::compute_intermediate_quaternion(const Quaternion& q_prev, + const Quaternion& q_curr, + const Quaternion& q_next) { + Quaternion q_inv = q_curr.inverse(); + + // Handle double-cover for both neighbors + Quaternion next = q_next; + if (q_curr.dot_product(q_next) < 0.0) next = -q_next; + Quaternion prev = q_prev; + if (q_curr.dot_product(q_prev) < 0.0) prev = -q_prev; + + Quaternion log_next = log(q_inv * next); + Quaternion log_prev = log(q_inv * prev); + + Quaternion sum = (log_next + log_prev) * (-0.25); + return q_curr * exp(sum); +} + +Eigen::Matrix3d Quaternion::to_rotation_matrix() const { + return q_.normalized().toRotationMatrix(); +} + +std::pair Quaternion::to_axis_angle() const { + Eigen::AngleAxisd aa(q_.normalized()); + double angle = aa.angle(); + Eigen::Vector3d axis = aa.axis(); + + if (angle < kEpsilon) { + return {Eigen::Vector3d::UnitX(), 0.0}; + } + return {axis, angle}; +} + +Eigen::Matrix4d Quaternion::to_transformation_matrix() const { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.block<3, 3>(0, 0) = to_rotation_matrix(); + return T; +} + +std::tuple Quaternion::to_euler_angles() const { + double w = q_.w(), x = q_.x(), y = q_.y(), z = q_.z(); + + // Roll (x-axis rotation) + double sinr_cosp = 2.0 * (w * x + y * z); + double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); + double roll = std::atan2(sinr_cosp, cosr_cosp); + + // Pitch (y-axis rotation) + double sinp = 2.0 * (w * y - z * x); + double pitch = (std::abs(sinp) >= 1.0) + ? std::copysign(M_PI / 2.0, sinp) + : std::asin(sinp); + + // Yaw (z-axis rotation) + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + + return {roll, pitch, yaw}; +} + +Quaternion Quaternion::from_rotation_matrix(const Eigen::Matrix3d& rotation_matrix) { + Eigen::Quaterniond eq(rotation_matrix); + eq.normalize(); + return Quaternion(eq); +} + +Eigen::Matrix3d Quaternion::E(int sign) const { + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + Eigen::Vector3d v = vec(); + + // Skew-symmetric matrix of v + Eigen::Matrix3d S; + S << 0, -v.z(), v.y(), + v.z(), 0, -v.x(), + -v.y(), v.x(), 0; + + if (sign == 1) { + // Body frame: E = sI + S(v) + return w() * I + S; + } + // Base frame: E = sI - S(v) + return w() * I - S; +} + +Quaternion Quaternion::dot(const Eigen::Vector3d& omega, int sign) const { + // Scalar derivative: s_dot = -0.5 * v^T * omega + double s_dot = -0.5 * vec().dot(omega); + + // Vector derivative: v_dot = 0.5 * E(sign) * omega + Eigen::Vector3d v_dot = 0.5 * E(sign) * omega; + + return Quaternion(s_dot, v_dot.x(), v_dot.y(), v_dot.z()); +} + +Eigen::Vector3d Quaternion::Omega(const Quaternion& q, const Quaternion& q_dot) { + Eigen::Matrix3d e_matrix = 0.5 * q.E(0); // Base frame + Eigen::Vector3d v_dot(q_dot.x(), q_dot.y(), q_dot.z()); + + double det = e_matrix.determinant(); + if (std::abs(det) > kEpsilon) { + return e_matrix.inverse() * v_dot; + } + // Fallback to pseudo-inverse for singular case + return e_matrix.completeOrthogonalDecomposition().solve(v_dot); +} + +} // namespace interpolatecpp::quat diff --git a/cpp/src/quaternion_spline.cpp b/cpp/src/quaternion_spline.cpp new file mode 100644 index 0000000..c7f9a5b --- /dev/null +++ b/cpp/src/quaternion_spline.cpp @@ -0,0 +1,101 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::quat { + +QuaternionSpline::QuaternionSpline(const std::vector& time_points, + const std::vector& quaternions, + Method method) + : times_(time_points), quaternions_(quaternions), method_(method) { + if (times_.size() != quaternions_.size()) { + throw std::invalid_argument("Time points and quaternions must have same length"); + } + if (times_.size() < 2) { + throw std::invalid_argument("Need at least 2 quaternions for interpolation"); + } + + compute_intermediates(); +} + +void QuaternionSpline::compute_intermediates() { + const int n = static_cast(quaternions_.size()); + intermediates_.resize(n); + + intermediates_[0] = quaternions_[0]; + intermediates_[n - 1] = quaternions_[n - 1]; + + for (int i = 1; i < n - 1; ++i) { + intermediates_[i] = Quaternion::compute_intermediate_quaternion( + quaternions_[i - 1], quaternions_[i], quaternions_[i + 1]); + } +} + +int QuaternionSpline::find_segment(double t) const { + const int n = static_cast(times_.size()); + if (t <= times_[0]) return 0; + if (t >= times_[n - 1]) return n - 2; + + auto it = std::upper_bound(times_.begin(), times_.end(), t); + int idx = static_cast(it - times_.begin()) - 1; + return std::clamp(idx, 0, n - 2); +} + +Quaternion QuaternionSpline::evaluate(double t) const { + t = std::clamp(t, times_.front(), times_.back()); + int seg = find_segment(t); + const int n = static_cast(quaternions_.size()); + + double u = (times_[seg + 1] - times_[seg]) > 1e-15 + ? (t - times_[seg]) / (times_[seg + 1] - times_[seg]) + : 0.0; + + bool use_squad = false; + if (method_ == Method::Squad || method_ == Method::Auto) { + use_squad = (n >= 4 && seg > 0 && seg < n - 2); + } + + if (use_squad) { + return Quaternion::squad(quaternions_[seg], intermediates_[seg], + intermediates_[seg + 1], quaternions_[seg + 1], u); + } + + return Quaternion::slerp(quaternions_[seg], quaternions_[seg + 1], u); +} + +Eigen::Vector3d QuaternionSpline::evaluate_velocity(double t) const { + double t_lo = std::max(t - kDt, times_.front()); + double t_hi = std::min(t + kDt, times_.back()); + double dt = t_hi - t_lo; + + if (dt < 1e-15) return Eigen::Vector3d::Zero(); + + Quaternion q_lo = evaluate(t_lo); + Quaternion q_hi = evaluate(t_hi); + + // dq/dt approximation + Quaternion dq = (q_hi - q_lo) * (1.0 / dt); + + // omega = 2 * dq * q^(-1), take vector part + Quaternion q_now = evaluate(t); + Quaternion omega_q = dq * q_now.inverse() * 2.0; + + return omega_q.vec(); +} + +Eigen::Vector3d QuaternionSpline::evaluate_acceleration(double t) const { + double t_lo = std::max(t - kDt, times_.front()); + double t_hi = std::min(t + kDt, times_.back()); + double dt = t_hi - t_lo; + + if (dt < 1e-15) return Eigen::Vector3d::Zero(); + + Eigen::Vector3d v_lo = evaluate_velocity(t_lo); + Eigen::Vector3d v_hi = evaluate_velocity(t_hi); + + return (v_hi - v_lo) / dt; +} + +} // namespace interpolatecpp::quat diff --git a/cpp/src/smoothing_cubic_bspline.cpp b/cpp/src/smoothing_cubic_bspline.cpp new file mode 100644 index 0000000..8d1955e --- /dev/null +++ b/cpp/src/smoothing_cubic_bspline.cpp @@ -0,0 +1,395 @@ +#include + +#include +#include +#include +#include + +namespace interpolatecpp::bspline { + +Eigen::VectorXd SmoothingCubicBSpline::calculate_parameters( + const Eigen::MatrixXd& points, Parameterization method) { + const int n = static_cast(points.rows()) - 1; + Eigen::VectorXd u_bars = Eigen::VectorXd::Zero(n + 1); + u_bars[0] = 0.0; + u_bars[n] = 1.0; + + switch (method) { + case Parameterization::EquallySpaced: { + for (int k = 1; k < n; ++k) { + u_bars[k] = static_cast(k) / static_cast(n); + } + break; + } + case Parameterization::ChordLength: { + double total = 0.0; + for (int k = 1; k <= n; ++k) { + total += (points.row(k) - points.row(k - 1)).norm(); + } + double accum = 0.0; + for (int k = 1; k < n; ++k) { + accum += (points.row(k) - points.row(k - 1)).norm(); + u_bars[k] = accum / total; + } + break; + } + case Parameterization::Centripetal: { + constexpr double mu = 0.5; + double total = 0.0; + for (int k = 1; k <= n; ++k) { + total += std::pow((points.row(k) - points.row(k - 1)).norm(), mu); + } + double accum = 0.0; + for (int k = 1; k < n; ++k) { + accum += std::pow((points.row(k) - points.row(k - 1)).norm(), mu); + u_bars[k] = accum / total; + } + break; + } + } + + return u_bars; +} + +Eigen::VectorXd SmoothingCubicBSpline::calculate_knot_vector() const { + const int n = static_cast(approximation_points_.rows()) - 1; + Eigen::VectorXd kv = Eigen::VectorXd::Zero(n + 7); + + kv[0] = u_bars_[0]; + kv[1] = u_bars_[0]; + kv[2] = u_bars_[0]; + + for (int j = 0; j <= n; ++j) { + kv[j + 3] = u_bars_[j]; + } + + kv[n + 4] = u_bars_[n]; + kv[n + 5] = u_bars_[n]; + kv[n + 6] = u_bars_[n]; + + return kv; +} + +Eigen::MatrixXd SmoothingCubicBSpline::construct_b_matrix() const { + const int n = static_cast(approximation_points_.rows()) - 1; + const int m = n + 2; // Last control point index (total m+1 = n+3) + + Eigen::MatrixXd b_mat = Eigen::MatrixXd::Zero(n + 1, m + 1); + + for (int k = 0; k <= n; ++k) { + const double u = u_bars_[k]; + const int span = find_knot_span(u); + const Eigen::VectorXd basis_vals = basis_functions(u, span); + + for (int j = 0; j <= degree_; ++j) { + b_mat(k, span - degree_ + j) = basis_vals[j]; + } + } + + return b_mat; +} + +Eigen::MatrixXd SmoothingCubicBSpline::construct_a_matrix() const { + const int n = static_cast(approximation_points_.rows()) - 1; + const int size = n + 1; + + Eigen::MatrixXd a_mat = Eigen::MatrixXd::Zero(size, size); + + for (int i = 0; i < size; ++i) { + int idx3 = i + 3; + int idx2 = i + 2; + int idx4 = i + 4; + + if (idx4 < static_cast(knots_.size())) { + double ui3_i2 = knots_[idx3] - knots_[idx2]; + double ui4_i3 = knots_[idx4] - knots_[idx3]; + + a_mat(i, i) = 2.0 * (ui3_i2 + ui4_i3); + + if (i < size - 1) { + a_mat(i, i + 1) = ui4_i3; + } + if (i > 0) { + a_mat(i, i - 1) = ui3_i2; + } + } + } + + return a_mat; +} + +Eigen::MatrixXd SmoothingCubicBSpline::construct_c_matrix() const { + const int n = static_cast(approximation_points_.rows()) - 1; + const int size_r = n + 1; + const int size_p = n + 3; + + Eigen::MatrixXd c_mat = Eigen::MatrixXd::Zero(size_r, size_p); + + for (int k = 0; k < size_r; ++k) { + int k4 = k + 4; + int k2 = k + 2; + int k1 = k + 1; + int k5 = k + 5; + + if (k4 < static_cast(knots_.size()) && + k5 < static_cast(knots_.size())) { + double uk4_k2 = knots_[k4] - knots_[k2]; + double uk4_k1 = knots_[k4] - knots_[k1]; + double uk5_k2 = knots_[k5] - knots_[k2]; + + if (std::abs(uk4_k2) > kEpsilon && std::abs(uk4_k1) > kEpsilon && + std::abs(uk5_k2) > kEpsilon) { + if (k < size_p) { + c_mat(k, k) = 6.0 / (uk4_k2 * uk4_k1); + } + if (k + 1 < size_p) { + c_mat(k, k + 1) = -6.0 / uk4_k2 * (1.0 / uk4_k1 + 1.0 / uk5_k2); + } + if (k + 2 < size_p) { + c_mat(k, k + 2) = 6.0 / (uk4_k2 * uk5_k2); + } + } + } + } + + return c_mat; +} + +Eigen::MatrixXd SmoothingCubicBSpline::calculate_control_points_impl() const { + if (enforce_endpoints_) { + return calculate_control_points_with_endpoints(); + } + + const int n = static_cast(approximation_points_.rows()) - 1; + const int dim = static_cast(approximation_points_.cols()); + + Eigen::MatrixXd b_mat = construct_b_matrix(); + Eigen::MatrixXd w_mat = weights_.asDiagonal(); + Eigen::MatrixXd a_mat = construct_a_matrix(); + Eigen::MatrixXd c_mat = construct_c_matrix(); + + Eigen::MatrixXd ctac = c_mat.transpose() * a_mat * c_mat; + Eigen::MatrixXd left = b_mat.transpose() * w_mat * b_mat + lambda_param_ * ctac; + Eigen::MatrixXd right = b_mat.transpose() * w_mat * approximation_points_; + + Eigen::MatrixXd cp = Eigen::MatrixXd::Zero(n + 3, dim); + + Eigen::FullPivLU lu(left); + if (lu.isInvertible()) { + for (int d = 0; d < dim; ++d) { + cp.col(d) = lu.solve(right.col(d)); + } + } else { + for (int d = 0; d < dim; ++d) { + cp.col(d) = left.completeOrthogonalDecomposition().solve(right.col(d)); + } + } + + return cp; +} + +Eigen::MatrixXd SmoothingCubicBSpline::calculate_control_points_with_endpoints() const { + const int n = static_cast(approximation_points_.rows()) - 1; + const int dim = static_cast(approximation_points_.cols()); + + Eigen::MatrixXd cp = Eigen::MatrixXd::Zero(n + 3, dim); + + cp.row(0) = approximation_points_.row(0); + cp.row(n + 2) = approximation_points_.row(n); + + double u4 = knots_[4]; + cp.row(1) = cp.row(0) + (u4 / 3.0) * v0_.transpose(); + + double un2 = knots_[n + 2]; + cp.row(n + 1) = cp.row(n + 2) - ((1.0 - un2) / 3.0) * vn_.transpose(); + + if (n <= 0) { + return cp; + } + + // Reduced system for p_2, ..., p_n + const int sys_size = n - 1; + Eigen::MatrixXd q_reduced = Eigen::MatrixXd::Zero(sys_size, dim); + for (int k = 1; k < n; ++k) { + q_reduced.row(k - 1) = approximation_points_.row(k); + } + + Eigen::MatrixXd b_reduced = Eigen::MatrixXd::Zero(sys_size, sys_size); + for (int k = 1; k < n; ++k) { + const double u = u_bars_[k]; + const int span = find_knot_span(u); + const Eigen::VectorXd basis_vals = basis_functions(u, span); + + // Subtract fixed control point contributions + if (span - degree_ <= 0) { + q_reduced.row(k - 1) -= basis_vals[0] * cp.row(0); + } + if (span - degree_ + 1 <= 1) { + q_reduced.row(k - 1) -= basis_vals[1] * cp.row(1); + } + if (span >= n) { + int basis_idx = n + 1 - (span - degree_); + if (basis_idx >= 0 && basis_idx < basis_vals.size()) { + q_reduced.row(k - 1) -= basis_vals[basis_idx] * cp.row(n + 1); + } + } + if (span >= n + 1) { + int basis_idx = n + 2 - (span - degree_); + if (basis_idx >= 0 && basis_idx < basis_vals.size()) { + q_reduced.row(k - 1) -= basis_vals[basis_idx] * cp.row(n + 2); + } + } + + // Fill B_reduced for p_2 to p_n + for (int j = 2; j <= n; ++j) { + int basis_idx = j - (span - degree_); + if (basis_idx >= 0 && basis_idx < basis_vals.size()) { + int col = j - 2; + if (col >= 0 && col < sys_size) { + b_reduced(k - 1, col) = basis_vals[basis_idx]; + } + } + } + } + + // Reduced weight matrix + Eigen::MatrixXd w_reduced = Eigen::MatrixXd::Zero(sys_size, sys_size); + for (int i = 0; i < sys_size; ++i) { + w_reduced(i, i) = weights_[i + 1]; + } + + Eigen::MatrixXd a_mat = construct_a_matrix(); + Eigen::MatrixXd c_mat = construct_c_matrix(); + Eigen::MatrixXd c_reduced = c_mat.block(0, 2, c_mat.rows(), sys_size); + + // PZ vector + Eigen::MatrixXd pz = Eigen::MatrixXd::Zero(n + 1, dim); + pz.row(0) = c_mat(0, 0) * cp.row(0) + c_mat(0, 1) * cp.row(1); + pz.row(n) = c_mat(n, n + 1) * cp.row(n + 1) + c_mat(n, n + 2) * cp.row(n + 2); + + Eigen::MatrixXd ctac_reduced = c_reduced.transpose() * a_mat * c_reduced; + Eigen::MatrixXd ctat_pz = c_reduced.transpose() * a_mat * pz; + + Eigen::MatrixXd left = b_reduced.transpose() * w_reduced * b_reduced + + lambda_param_ * ctac_reduced; + Eigen::MatrixXd right_side = + b_reduced.transpose() * w_reduced * q_reduced - lambda_param_ * ctat_pz; + + Eigen::FullPivLU lu(left); + if (lu.isInvertible()) { + for (int d = 0; d < dim; ++d) { + cp.block(2, d, sys_size, 1) = lu.solve(right_side.col(d)); + } + } else { + for (int d = 0; d < dim; ++d) { + cp.block(2, d, sys_size, 1) = + left.completeOrthogonalDecomposition().solve(right_side.col(d)); + } + } + + return cp; +} + +Eigen::VectorXd SmoothingCubicBSpline::calculate_approximation_error() const { + const int n = static_cast(approximation_points_.rows()); + Eigen::VectorXd errors = Eigen::VectorXd::Zero(n); + + for (int k = 0; k < n; ++k) { + Eigen::VectorXd pt = evaluate(u_bars_[k]); + errors[k] = (pt - approximation_points_.row(k).transpose()).norm(); + } + + return errors; +} + +double SmoothingCubicBSpline::calculate_total_error() const { + double total = 0.0; + const int n = static_cast(approximation_points_.rows()); + + for (int k = 0; k < n; ++k) { + Eigen::VectorXd pt = evaluate(u_bars_[k]); + Eigen::VectorXd diff = pt - approximation_points_.row(k).transpose(); + total += weights_[k] * diff.squaredNorm(); + } + + return total; +} + +SmoothingCubicBSpline::SmoothingCubicBSpline(const Eigen::MatrixXd& points, + const BSplineParams& params) + : BSpline(DeferInit{}) +{ + Eigen::MatrixXd pts = points; + const int n_points = static_cast(pts.rows()); + const int dim = static_cast(pts.cols()); + const int n = n_points - 1; + + approximation_points_ = pts; + mu_ = std::clamp(params.mu, 0.0, 1.0); + lambda_param_ = (mu_ > 0.0) ? (1.0 - mu_) / (6.0 * mu_) + : std::numeric_limits::infinity(); + enforce_endpoints_ = params.enforce_endpoints; + + // Set weights + if (params.weights.has_value()) { + if (params.weights->size() != n_points) { + throw std::invalid_argument( + "Length of weights must match the number of points"); + } + weights_ = params.weights.value(); + } else { + weights_ = Eigen::VectorXd::Ones(n_points); + } + + // Calculate parameters + u_bars_ = calculate_parameters(pts, params.method); + + // Calculate knot vector + Eigen::VectorXd kv = calculate_knot_vector(); + + // Process endpoint derivatives + if (params.enforce_endpoints) { + if (params.v0.has_value()) { + v0_ = params.v0.value(); + } else if (params.auto_derivatives && n > 0) { + double u_diff = u_bars_[1] - u_bars_[0]; + if (std::abs(u_diff) > kEpsilon) { + v0_ = (pts.row(1) - pts.row(0)).transpose() / u_diff; + } else { + v0_ = Eigen::VectorXd::Zero(dim); + } + } else { + v0_ = Eigen::VectorXd::Zero(dim); + } + + if (params.vn.has_value()) { + vn_ = params.vn.value(); + } else if (params.auto_derivatives && n > 0) { + double u_diff = u_bars_[n] - u_bars_[n - 1]; + if (std::abs(u_diff) > kEpsilon) { + vn_ = (pts.row(n) - pts.row(n - 1)).transpose() / u_diff; + } else { + vn_ = Eigen::VectorXd::Zero(dim); + } + } else { + vn_ = Eigen::VectorXd::Zero(dim); + } + } else { + v0_ = Eigen::VectorXd::Zero(dim); + vn_ = Eigen::VectorXd::Zero(dim); + } + + // Initialize base with knot vector to enable basis_functions() + degree_ = 3; + knots_ = kv; + const int n_cp = n + 3; + control_points_ = Eigen::MatrixXd::Zero(n_cp, dim); + u_min_ = knots_[3]; + u_max_ = knots_[static_cast(knots_.size()) - 4]; + dimension_ = dim; + + // Now calculate actual control points + control_points_ = calculate_control_points_impl(); +} + +} // namespace interpolatecpp::bspline diff --git a/cpp/src/smoothing_search.cpp b/cpp/src/smoothing_search.cpp new file mode 100644 index 0000000..f6b5467 --- /dev/null +++ b/cpp/src/smoothing_search.cpp @@ -0,0 +1,96 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::spline { + +namespace { +constexpr double kEpsilon = 1e-6; +} + +SmoothingSearchResult smoothing_spline_with_tolerance(std::span t_points, + std::span q_points, + double tolerance, + const SplineConfig& config) { + double lower_bound = kEpsilon; + double upper_bound = 1.0; + + if (config.debug) { + std::cout << "Starting binary search with tolerance delta=" << tolerance << "\n"; + std::cout << "Initial lower_bound=" << lower_bound + << ", upper_bound=" << upper_bound << "\n"; + } + + // Weights span (may be empty) + std::optional> weights_span; + if (config.weights.has_value()) { + const auto& w = config.weights.value(); + weights_span = std::span(w.data(), static_cast(w.size())); + } + + // Default fallback: mu=1.0 + CubicSmoothingSpline default_spline(t_points, q_points, 1.0, weights_span, config.v0, + config.vn, false); + double default_error = + (default_spline.q_points() - default_spline.s_points()).cwiseAbs().maxCoeff(); + + CubicSmoothingSpline best_spline = default_spline; + double best_mu = 1.0; + double best_error = default_error; + + for (int i = 0; i < config.max_iterations; ++i) { + double mu = (lower_bound + upper_bound) / 2.0; + + if (config.debug) { + std::cout << "\nIteration " << (i + 1) << ": mu=" << mu << "\n"; + } + + try { + CubicSmoothingSpline spline(t_points, q_points, mu, weights_span, config.v0, + config.vn, false); + double e_max = (spline.q_points() - spline.s_points()).cwiseAbs().maxCoeff(); + + if (config.debug) { + std::cout << " Maximum error e_max(" << i << ")=" << e_max << "\n"; + } + + if (e_max < best_error) { + best_spline = spline; + best_mu = mu; + best_error = e_max; + } + + if (e_max > tolerance) { + lower_bound = mu; + } else { + upper_bound = mu; + } + + // Convergence check + if (std::abs(e_max - tolerance) < kEpsilon || + (e_max < tolerance && upper_bound - lower_bound < kEpsilon)) { + if (config.debug) { + std::cout << "\nConverged with error " << e_max << " after " << (i + 1) + << " iterations\n"; + } + return {std::move(spline), mu, e_max, i + 1}; + } + } catch (const std::exception& e) { + if (config.debug) { + std::cout << " Error with mu=" << mu << ": " << e.what() << "\n"; + } + lower_bound = mu; + } + } + + if (config.debug) { + std::cout << "\nReached maximum iterations (" << config.max_iterations << ")\n"; + std::cout << "Best solution found: mu=" << best_mu << ", error=" << best_error << "\n"; + } + + return {std::move(best_spline), best_mu, best_error, config.max_iterations}; +} + +} // namespace interpolatecpp::spline diff --git a/cpp/src/squad_c2.cpp b/cpp/src/squad_c2.cpp new file mode 100644 index 0000000..e2a4c31 --- /dev/null +++ b/cpp/src/squad_c2.cpp @@ -0,0 +1,192 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::quat { + +double SquadC2::quintic_u(double t, double t0, double t1) { + // Quintic polynomial with zero-clamped boundary conditions: + // u(t0)=0, u'(t0)=0, u''(t0)=0, u(t1)=1, u'(t1)=0, u''(t1)=0 + double h = t1 - t0; + if (h < 1e-15) return 0.0; + + double s = (t - t0) / h; + s = std::clamp(s, 0.0, 1.0); + + // u(s) = 10sยณ - 15sโด + 6sโต + return s * s * s * (10.0 - 15.0 * s + 6.0 * s * s); +} + +void SquadC2::build_extended_sequence() { + const int n = static_cast(quaternions_.size()); + if (n < 2) return; + + // Extended sequence: [qโ‚, qโ‚แต›โฑสณแต—, qโ‚‚, ..., qโ‚™โ‚‹โ‚, qโ‚™โ‚‹โ‚แต›โฑสณแต—, qโ‚™] + ext_quats_.clear(); + ext_times_.clear(); + + // First original point + ext_quats_.push_back(quaternions_[0]); + ext_times_.push_back(times_[0]); + + // First virtual = first original quaternion + if (n == 2) { + // Special case: dt/3 spacing for 2-waypoint case + double dt = times_[1] - times_[0]; + ext_quats_.push_back(quaternions_[0]); + ext_times_.push_back(times_[0] + dt / 3.0); + } else { + // General case: midpoint spacing + double t_mid = (times_[0] + times_[1]) / 2.0; + ext_quats_.push_back(quaternions_[0]); + ext_times_.push_back(t_mid); + } + + // Interior original points + for (int i = 1; i < n - 1; ++i) { + ext_quats_.push_back(quaternions_[i]); + ext_times_.push_back(times_[i]); + } + + // Last virtual = last original quaternion + if (n == 2) { + // Special case: dt/3 spacing for 2-waypoint case + double dt = times_[1] - times_[0]; + ext_quats_.push_back(quaternions_[n - 1]); + ext_times_.push_back(times_[1] - dt / 3.0); + } else { + // General case: midpoint spacing + double t_mid2 = (times_[n - 2] + times_[n - 1]) / 2.0; + ext_quats_.push_back(quaternions_[n - 1]); + ext_times_.push_back(t_mid2); + } + + // Last original point + ext_quats_.push_back(quaternions_[n - 1]); + ext_times_.push_back(times_[n - 1]); +} + +void SquadC2::compute_intermediates() { + const int n_ext = static_cast(ext_quats_.size()); + ext_intermediates_.resize(n_ext); + + ext_intermediates_[0] = ext_quats_[0]; + ext_intermediates_[n_ext - 1] = ext_quats_[n_ext - 1]; + + for (int i = 1; i < n_ext - 1; ++i) { + double h_prev = ext_times_[i] - ext_times_[i - 1]; + double h_curr = ext_times_[i + 1] - ext_times_[i]; + + Quaternion q_inv = ext_quats_[i].inverse(); + + // Handle double-cover + Quaternion next = ext_quats_[i + 1]; + if (ext_quats_[i].dot_product(next) < 0.0) next = -next; + Quaternion prev = ext_quats_[i - 1]; + if (ext_quats_[i].dot_product(prev) < 0.0) prev = -prev; + + Quaternion log_next = Quaternion::log(q_inv * next); + Quaternion log_prev = Quaternion::log(q_inv * prev); + + // Corrected Wittmann formula for non-uniform spacing + double w_next = -1.0 / (2.0 * (1.0 + h_curr / std::max(h_prev, 1e-15))); + double w_prev = -1.0 / (2.0 * (1.0 + h_prev / std::max(h_curr, 1e-15))); + + Quaternion weighted_sum = log_next * w_next + log_prev * w_prev; + ext_intermediates_[i] = ext_quats_[i] * Quaternion::exp(weighted_sum); + } +} + +int SquadC2::find_original_segment(double t) const { + const int n = static_cast(times_.size()); + if (t <= times_[0]) return 0; + if (t >= times_[n - 1]) return n - 2; + + auto it = std::upper_bound(times_.begin(), times_.end(), t); + int idx = static_cast(it - times_.begin()) - 1; + return std::clamp(idx, 0, n - 2); +} + +SquadC2::SquadC2(const std::vector& time_points, + const std::vector& quaternions, bool normalize_quaternions, + bool validate_continuity) + : times_(time_points), validate_continuity_(validate_continuity) { + if (time_points.size() != quaternions.size()) { + throw std::invalid_argument("Time points and quaternions must have same length"); + } + if (quaternions.size() < 2) { + throw std::invalid_argument("Need at least 2 quaternions"); + } + for (size_t i = 1; i < time_points.size(); ++i) { + if (time_points[i] <= time_points[i - 1]) { + throw std::invalid_argument("Time points must be strictly increasing"); + } + } + + if (normalize_quaternions) { + quaternions_.reserve(quaternions.size()); + for (const auto& q : quaternions) { + quaternions_.push_back(q.unit()); + } + } else { + quaternions_ = quaternions; + } + + build_extended_sequence(); + compute_intermediates(); +} + +SquadC2::SquadC2(const SquadC2Config& config) + : SquadC2(config.time_points, config.quaternions, config.normalize_quaternions, + config.validate_continuity) {} + +Quaternion SquadC2::evaluate(double t) const { + // Boundary cases โ€” return original endpoint quaternions + if (t <= times_.front()) return quaternions_.front(); + if (t >= times_.back()) return quaternions_.back(); + + // Find segment in ORIGINAL time points + int seg = find_original_segment(t); + + // Quintic parameterization over the original segment + double u = quintic_u(t, times_[seg], times_[seg + 1]); + + // Map original segment index to extended sequence for SQUAD: + // q1, q2 from original waypoints; s1, s2 from extended intermediates. + // Extended layout: [qโ‚, qโ‚แต›โฑสณแต—, qโ‚‚, ..., qโ‚™โ‚‹โ‚, qโ‚™แต›โฑสณแต—, qโ‚™] + // For original segment i: s1 = ext_intermediates_[i+1], s2 = ext_intermediates_[i+2] + const Quaternion& q1 = quaternions_[seg]; + const Quaternion& q2 = quaternions_[seg + 1]; + const Quaternion& s1 = ext_intermediates_[static_cast(seg) + 1]; + const Quaternion& s2 = ext_intermediates_[static_cast(seg) + 2]; + + return Quaternion::squad(q1, s1, s2, q2, u); +} + +Eigen::Vector3d SquadC2::evaluate_velocity(double t) const { + double t_lo = std::max(t - kDt, times_.front()); + double t_hi = std::min(t + kDt, times_.back()); + double dt = t_hi - t_lo; + if (dt < 1e-15) return Eigen::Vector3d::Zero(); + + Quaternion q_lo = evaluate(t_lo); + Quaternion q_hi = evaluate(t_hi); + Quaternion q_now = evaluate(t); + + Quaternion dq = (q_hi - q_lo) * (1.0 / dt); + Quaternion omega_q = dq * q_now.inverse() * 2.0; + return omega_q.vec(); +} + +Eigen::Vector3d SquadC2::evaluate_acceleration(double t) const { + double t_lo = std::max(t - kDt, times_.front()); + double t_hi = std::min(t + kDt, times_.back()); + double dt = t_hi - t_lo; + if (dt < 1e-15) return Eigen::Vector3d::Zero(); + + return (evaluate_velocity(t_hi) - evaluate_velocity(t_lo)) / dt; +} + +} // namespace interpolatecpp::quat diff --git a/cpp/src/trapezoidal_trajectory.cpp b/cpp/src/trapezoidal_trajectory.cpp new file mode 100644 index 0000000..ca54b33 --- /dev/null +++ b/cpp/src/trapezoidal_trajectory.cpp @@ -0,0 +1,252 @@ +#include + +#include +#include +#include + +namespace interpolatecpp::motion { + +void TrapezoidalTrajectory::plan_velocity_based(double amax, double vmax) { + double h = std::abs(q1_ - q0_); + // Work in positive displacement space + double q0 = (sign_ > 0) ? q0_ : -q0_; + double q1 = (sign_ > 0) ? q1_ : -q1_; + double v0 = (sign_ > 0) ? v0_ : -v0_; + double v1 = (sign_ > 0) ? v1_ : -v1_; + + h = q1 - q0; + + if (h * amax > vmax * vmax - (v0 * v0 + v1 * v1) / 2.0) { + // vmax is reached + vv_ = vmax; + ta_ = (vmax - v0) / (amax + kEpsilon); + td_ = (vmax - v1) / (amax + kEpsilon); + + double v0r = v0 / std::max(vmax, kEpsilon); + double v1r = v1 / std::max(vmax, kEpsilon); + v0r = std::clamp(v0r, -1.0 + kEpsilon, 1.0 - kEpsilon); + v1r = std::clamp(v1r, -1.0 + kEpsilon, 1.0 - kEpsilon); + + duration_ = h / std::max(vmax, kEpsilon) + + vmax / (2.0 * amax + kEpsilon) * (1.0 - v0r) * (1.0 - v0r) + + vmax / (2.0 * amax + kEpsilon) * (1.0 - v1r) * (1.0 - v1r); + } else { + // Triangular profile + double sqrt_term = h * amax + (v0 * v0 + v1 * v1) / 2.0; + if (sqrt_term < 0) { + sqrt_term = std::max(sqrt_term, 0.0); + } + double vlim = std::sqrt(sqrt_term); + vv_ = vlim; + ta_ = (vlim - v0) / (amax + kEpsilon); + td_ = (vlim - v1) / (amax + kEpsilon); + duration_ = ta_ + td_; + } +} + +void TrapezoidalTrajectory::plan_duration_based(double amax, double total_duration) { + double q0 = (sign_ > 0) ? q0_ : -q0_; + double q1 = (sign_ > 0) ? q1_ : -q1_; + double v0 = (sign_ > 0) ? v0_ : -v0_; + double v1 = (sign_ > 0) ? v1_ : -v1_; + + double h = q1 - q0; + + // Feasibility check + if (amax * h < std::abs(v0 * v0 - v1 * v1) / 2.0) { + throw std::invalid_argument( + "Trajectory not feasible. Try increasing amax or reducing velocities."); + } + + double T2 = total_duration * total_duration; + double sqrt_term = + 4.0 * h * h - 4.0 * h * (v0 + v1) * total_duration + + 2.0 * (v0 * v0 + v1 * v1) * T2; + + if (sqrt_term < 0) { + if (sqrt_term > -kEpsilon) { + sqrt_term = 0.0; + } else { + throw std::invalid_argument( + "Trajectory not feasible with given duration."); + } + } + + double alim = + (2.0 * h - total_duration * (v0 + v1) + std::sqrt(sqrt_term)) / + std::max(T2, kEpsilon); + + if (amax < alim) { + amax = alim; + } + + double vv_sqrt = + amax * amax * T2 - 4.0 * amax * h + + 2.0 * amax * (v0 + v1) * total_duration - (v0 - v1) * (v0 - v1); + + if (vv_sqrt < 0) { + vv_sqrt = std::max(vv_sqrt, 0.0); + } + + vv_ = 0.5 * (v0 + v1 + amax * total_duration - std::sqrt(vv_sqrt)); + ta_ = (vv_ - v0) / (amax + kEpsilon); + td_ = (vv_ - v1) / (amax + kEpsilon); + duration_ = total_duration; +} + +TrapezoidalTrajectory::TrapezoidalTrajectory(double q0, double q1, double amax, + double vmax, double v0, double v1, + double t0) + : q0_(q0), q1_(q1), v0_(v0), v1_(v1), t0_(t0), duration_(0), ta_(0), td_(0), + vv_(0) { + amax = std::abs(amax); + vmax = std::abs(vmax); + + double h = q1 - q0; + sign_ = (h >= 0) ? 1 : -1; + + plan_velocity_based(amax, vmax); +} + +TrapezoidalTrajectory::TrapezoidalTrajectory(DurationBased, double q0, double q1, + double amax, double v0, double v1, + double t0, double duration) + : q0_(q0), q1_(q1), v0_(v0), v1_(v1), t0_(t0), duration_(0), ta_(0), td_(0), + vv_(0), sign_(1) { + amax = std::abs(amax); + + double h = q1 - q0; + sign_ = (h >= 0) ? 1 : -1; + + plan_duration_based(amax, duration); +} + +TrajectoryResult TrapezoidalTrajectory::evaluate(double t) const { + double t1 = t0_ + duration_; + t = std::clamp(t, t0_, t1); + + // Work in transformed space + double q0 = (sign_ > 0) ? q0_ : -q0_; + double q1 = (sign_ > 0) ? q1_ : -q1_; + double v0 = (sign_ > 0) ? v0_ : -v0_; + double v1 = (sign_ > 0) ? v1_ : -v1_; + + double ta_safe = std::max(ta_, kEpsilon); + double td_safe = std::max(td_, kEpsilon); + + double pos = 0.0, vel = 0.0, acc = 0.0; + + if (t < t0_ + ta_safe) { + double dt = t - t0_; + pos = q0 + v0 * dt + (vv_ - v0) / (2.0 * ta_safe) * dt * dt; + vel = v0 + (vv_ - v0) / ta_safe * dt; + acc = (vv_ - v0) / ta_safe; + } else if (t < t1 - td_safe) { + pos = q0 + v0 * ta_safe / 2.0 + vv_ * (t - t0_ - ta_safe / 2.0); + vel = vv_; + acc = 0.0; + } else { + double dt = t1 - t; + pos = q1 - v1 * dt - (vv_ - v1) / (2.0 * td_safe) * dt * dt; + vel = v1 + (vv_ - v1) / td_safe * dt; + acc = -(vv_ - v1) / td_safe; + } + + if (sign_ < 0) { + return {-pos, -vel, -acc}; + } + return {pos, vel, acc}; +} + +std::vector TrapezoidalTrajectory::heuristic_velocities( + const std::vector& points, const std::vector& /*times*/, + double vmax) { + const int n = static_cast(points.size()); + std::vector velocities(n, 0.0); + + if (n < 2) return velocities; + + std::vector h_values(n - 1); + for (int k = 0; k < n - 1; ++k) { + h_values[k] = points[k + 1] - points[k]; + } + + for (int k = 0; k < n - 2; ++k) { + if (h_values[k] * h_values[k + 1] <= 0) { + velocities[k + 1] = 0.0; + } else { + velocities[k + 1] = (h_values[k] > 0) ? vmax : -vmax; + } + } + + return velocities; +} + +std::vector TrapezoidalTrajectory::interpolate_waypoints( + const std::vector& points, double amax, double vmax, double v0, + double vn, const std::vector& times, + const std::vector& velocities) { + const int n = static_cast(points.size()); + if (n < 2) { + throw std::invalid_argument("Need at least 2 points for interpolation"); + } + + std::vector vels; + if (!velocities.empty()) { + vels.resize(n, 0.0); + vels[0] = v0; + vels[n - 1] = vn; + for (int i = 0; i < static_cast(velocities.size()) && i < n - 2; ++i) { + vels[i + 1] = velocities[i]; + } + } else { + vels = heuristic_velocities(points, times, vmax); + vels[0] = v0; + vels[n - 1] = vn; + } + + std::vector segments; + segments.reserve(n - 1); + + double t_current = 0.0; + for (int i = 0; i < n - 1; ++i) { + if (!times.empty() && times.size() >= static_cast(n)) { + double seg_duration = times[i + 1] - times[i]; + segments.emplace_back(DurationBased{}, points[i], points[i + 1], amax, + vels[i], vels[i + 1], t_current, seg_duration); + } else { + segments.emplace_back(points[i], points[i + 1], amax, vmax, vels[i], + vels[i + 1], t_current); + } + t_current += segments.back().duration(); + } + + return segments; +} + +TrajectoryResult TrapezoidalTrajectory::evaluate_multipoint( + const std::vector& segments, double t) { + if (segments.empty()) return {0.0, 0.0, 0.0}; + + if (t <= segments.front().t_start()) { + return segments.front().evaluate(segments.front().t_start()); + } + if (t >= segments.back().t_end()) { + return segments.back().evaluate(segments.back().t_end()); + } + + // Binary search + int lo = 0, hi = static_cast(segments.size()) - 1; + while (lo < hi) { + int mid = (lo + hi) / 2; + if (t >= segments[mid].t_end()) { + lo = mid + 1; + } else { + hi = mid; + } + } + + return segments[lo].evaluate(t); +} + +} // namespace interpolatecpp::motion diff --git a/cpp/tests/CMakeLists.txt b/cpp/tests/CMakeLists.txt new file mode 100644 index 0000000..b2bf83e --- /dev/null +++ b/cpp/tests/CMakeLists.txt @@ -0,0 +1,49 @@ +include(Catch2Testing OPTIONAL RESULT_VARIABLE CATCH2_TESTING_MODULE) +if(NOT CATCH2_TESTING_MODULE) + list(APPEND CMAKE_MODULE_PATH ${Catch2_SOURCE_DIR}/extras) +endif() +include(CTest) +include(Catch) + +set(TEST_SOURCES + test_tridiagonal.cpp + test_cubic_spline.cpp + test_cubic_smoothing_spline.cpp + test_cubic_spline_with_acc1.cpp + test_cubic_spline_with_acc2.cpp + test_smoothing_search.cpp + test_concepts.cpp + # Phase 2: B-spline + test_bspline.cpp + test_bspline_variants.cpp + # Phase 3: Motion + test_polynomial_trajectory.cpp + test_double_s_trajectory.cpp + test_trapezoidal_trajectory.cpp + test_parabolic_blend_trajectory.cpp + # Phase 4: Quaternion + test_quaternion.cpp + test_quaternion_spline.cpp + # Phase 5: Path + test_paths.cpp +) + +add_executable(interpolatecpp_tests ${TEST_SOURCES}) + +target_link_libraries(interpolatecpp_tests + PRIVATE + interpolatecpp::interpolatecpp + Catch2::Catch2WithMain +) + +target_include_directories(interpolatecpp_tests + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/shared +) + +target_compile_options(interpolatecpp_tests PRIVATE + $<$: + -Wall -Wextra -Wpedantic + > +) + +catch_discover_tests(interpolatecpp_tests) diff --git a/cpp/tests/shared/test_data.hpp b/cpp/tests/shared/test_data.hpp new file mode 100644 index 0000000..7cd546a --- /dev/null +++ b/cpp/tests/shared/test_data.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +namespace interpolatecpp::test { + +// Tolerance levels matching Python test conventions +inline constexpr double kRegularRtol = 1e-10; +inline constexpr double kRegularAtol = 1e-10; +inline constexpr double kNumericalRtol = 1e-6; +inline constexpr double kNumericalAtol = 1e-6; +inline constexpr double kLooseAtol = 1e-4; + +// Common test data: basic waypoints +inline const Eigen::VectorXd kBasicTimes = (Eigen::VectorXd(4) << 0.0, 1.0, 2.0, 3.0).finished(); +inline const Eigen::VectorXd kBasicPositions = + (Eigen::VectorXd(4) << 0.0, 1.0, 0.0, 1.0).finished(); + +// Linear function test data: y = 2x +inline const Eigen::VectorXd kLinearTimes = + (Eigen::VectorXd(4) << 0.0, 1.0, 2.0, 3.0).finished(); +inline const Eigen::VectorXd kLinearPositions = + (Eigen::VectorXd(4) << 0.0, 2.0, 4.0, 6.0).finished(); + +// Quadratic function test data: y = x^2 +inline const Eigen::VectorXd kQuadraticTimes = + (Eigen::VectorXd(4) << 0.0, 1.0, 2.0, 3.0).finished(); +inline const Eigen::VectorXd kQuadraticPositions = + (Eigen::VectorXd(4) << 0.0, 1.0, 4.0, 9.0).finished(); + +// Symmetric trajectory +inline const Eigen::VectorXd kSymmetricTimes = + (Eigen::VectorXd(5) << 0.0, 1.0, 2.0, 3.0, 4.0).finished(); +inline const Eigen::VectorXd kSymmetricPositions = + (Eigen::VectorXd(5) << 0.0, 1.0, 2.0, 1.0, 0.0).finished(); + +} // namespace interpolatecpp::test diff --git a/cpp/tests/test_bspline.cpp b/cpp/tests/test_bspline.cpp new file mode 100644 index 0000000..6eb8693 --- /dev/null +++ b/cpp/tests/test_bspline.cpp @@ -0,0 +1,449 @@ +#include +#include + +#include +#include "test_data.hpp" + +using namespace interpolatecpp::bspline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +// --- Construction --- + +TEST_CASE("BSpline construction", "[bspline]") { + SECTION("Basic 1D construction") { + std::vector knots = {0, 0, 0, 1, 2, 3, 3, 3}; + Eigen::MatrixXd cp(5, 1); + cp << 1, 2, 3, 4, 5; + BSpline bs(2, knots, cp); + + REQUIRE(bs.degree() == 2); + REQUIRE(bs.knots().size() == 8); + REQUIRE(bs.n_control_points() == 5); + REQUIRE(bs.dimension() == 1); + REQUIRE_THAT(bs.u_min(), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(bs.u_max(), WithinAbs(3.0, kRegularAtol)); + } + + SECTION("Basic 2D construction") { + std::vector knots = {0, 0, 0, 1, 2, 3, 3, 3}; + Eigen::MatrixXd cp(5, 2); + cp << 0, 0, 1, 1, 2, 0, 3, 1, 4, 0; + BSpline bs(2, knots, cp); + + REQUIRE(bs.dimension() == 2); + REQUIRE(bs.control_points().rows() == 5); + REQUIRE(bs.control_points().cols() == 2); + } + + SECTION("Basic 3D construction") { + std::vector knots = {0, 0, 1, 2, 2}; + Eigen::MatrixXd cp(3, 3); + cp << 0, 0, 0, 1, 1, 1, 2, 0, 2; + BSpline bs(1, knots, cp); + + REQUIRE(bs.dimension() == 3); + REQUIRE(bs.control_points().rows() == 3); + } + + SECTION("Degree zero construction") { + std::vector knots = {0, 1, 2, 3}; + Eigen::MatrixXd cp(3, 1); + cp << 1, 2, 3; + BSpline bs(0, knots, cp); + + REQUIRE(bs.degree() == 0); + REQUIRE_THAT(bs.u_min(), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(bs.u_max(), WithinAbs(3.0, kRegularAtol)); + } + + SECTION("High degree construction") { + std::vector knots = {0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1}; + Eigen::MatrixXd cp(6, 2); + for (int i = 0; i < 6; ++i) { + cp(i, 0) = static_cast(i); + cp(i, 1) = static_cast(i * i); + } + BSpline bs(5, knots, cp); + + REQUIRE(bs.degree() == 5); + REQUIRE(bs.dimension() == 2); + } +} + +TEST_CASE("BSpline construction validation", "[bspline]") { + SECTION("Negative degree") { + std::vector knots = {0, 0, 1, 1}; + Eigen::MatrixXd cp(2, 1); + cp << 1, 2; + REQUIRE_THROWS_AS(BSpline(-1, knots, cp), std::invalid_argument); + } + + SECTION("Non-decreasing knots") { + std::vector knots = {0, 1, 0.5, 1}; + Eigen::MatrixXd cp(2, 1); + cp << 1, 2; + REQUIRE_THROWS_AS(BSpline(1, knots, cp), std::invalid_argument); + } + + SECTION("Invalid knot relationship") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(4, 1); + cp << 1, 2, 3, 4; + REQUIRE_THROWS_AS(BSpline(2, knots, cp), std::invalid_argument); + } +} + +// --- Knot span --- + +TEST_CASE("BSpline knot span", "[bspline]") { + std::vector knots = {0, 0, 0, 1, 2, 3, 3, 3}; + Eigen::MatrixXd cp(5, 1); + cp << 1, 2, 3, 4, 5; + BSpline bs(2, knots, cp); + + SECTION("Basic knot span finding") { + REQUIRE(bs.find_knot_span(0.0) == 2); + REQUIRE(bs.find_knot_span(0.5) == 2); + REQUIRE(bs.find_knot_span(1.0) == 3); + REQUIRE(bs.find_knot_span(1.5) == 3); + REQUIRE(bs.find_knot_span(2.0) == 4); + REQUIRE(bs.find_knot_span(3.0) == 4); + } + + SECTION("Boundary conditions") { + REQUIRE(bs.find_knot_span(bs.u_min()) == bs.degree()); + REQUIRE(bs.find_knot_span(bs.u_max()) == + static_cast(bs.knots().size()) - bs.degree() - 2); + } + + SECTION("Out of range") { + REQUIRE_THROWS_AS(bs.find_knot_span(-1.0), std::invalid_argument); + REQUIRE_THROWS_AS(bs.find_knot_span(4.0), std::invalid_argument); + } +} + +// --- Basis functions --- + +TEST_CASE("BSpline basis functions", "[bspline]") { + SECTION("Degree zero") { + std::vector knots = {0, 1, 2, 3}; + Eigen::MatrixXd cp(3, 1); + cp << 1, 2, 3; + BSpline bs(0, knots, cp); + + auto basis = bs.basis_functions(0.5, bs.find_knot_span(0.5)); + REQUIRE(basis.size() == 1); + REQUIRE_THAT(basis.sum(), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Degree one") { + std::vector knots = {0, 0, 1, 2, 2}; + Eigen::MatrixXd cp(3, 1); + cp << 1, 2, 3; + BSpline bs(1, knots, cp); + + auto basis = bs.basis_functions(0.5, bs.find_knot_span(0.5)); + REQUIRE(basis.size() == 2); + REQUIRE_THAT(basis.sum(), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Degree two") { + std::vector knots = {0, 0, 0, 1, 2, 2, 2}; + Eigen::MatrixXd cp(4, 1); + cp << 1, 2, 3, 4; + BSpline bs(2, knots, cp); + + auto basis = bs.basis_functions(0.5, bs.find_knot_span(0.5)); + REQUIRE(basis.size() == 3); + REQUIRE_THAT(basis.sum(), WithinAbs(1.0, kNumericalAtol)); + for (int i = 0; i < basis.size(); ++i) { + REQUIRE(basis[i] >= -kNumericalAtol); + } + } + + SECTION("Partition of unity") { + std::vector knots = {0, 0, 0, 0, 1, 2, 3, 3, 3, 3}; + Eigen::MatrixXd cp(6, 1); + cp << 1, 2, 3, 4, 5, 6; + BSpline bs(3, knots, cp); + + std::vector test_u = {0.0, 0.3, 0.7, 1.0, 1.5, 2.0, 2.5, 3.0}; + for (double u : test_u) { + int span = bs.find_knot_span(u); + auto basis = bs.basis_functions(u, span); + REQUIRE_THAT(basis.sum(), WithinAbs(1.0, kNumericalAtol)); + for (int i = 0; i < basis.size(); ++i) { + REQUIRE(basis[i] >= -kNumericalAtol); + } + } + } + + SECTION("Basis function derivatives") { + std::vector knots = {0, 0, 0, 1, 2, 2, 2}; + Eigen::MatrixXd cp(4, 1); + cp << 1, 2, 3, 4; + BSpline bs(2, knots, cp); + + int span = bs.find_knot_span(0.5); + auto ders = bs.basis_function_derivatives(0.5, span, 2); + + REQUIRE(ders.rows() == 3); // orders 0, 1, 2 + REQUIRE(ders.cols() == 3); // degree+1 basis functions + + // Zero-th derivative row should equal basis functions + auto basis = bs.basis_functions(0.5, span); + for (int j = 0; j < 3; ++j) { + REQUIRE_THAT(ders(0, j), WithinAbs(basis[j], kNumericalAtol)); + } + } + + SECTION("First derivatives sum to zero") { + std::vector knots = {0, 0, 0, 1, 2, 3, 3, 3}; + Eigen::MatrixXd cp(5, 1); + cp << 1, 2, 3, 4, 5; + BSpline bs(2, knots, cp); + + int span = bs.find_knot_span(1.5); + auto ders = bs.basis_function_derivatives(1.5, span, 1); + + double sum_d1 = 0.0; + for (int j = 0; j <= bs.degree(); ++j) { + sum_d1 += ders(1, j); + } + REQUIRE_THAT(std::abs(sum_d1), WithinAbs(0.0, kNumericalAtol)); + } +} + +// --- Evaluation --- + +TEST_CASE("BSpline evaluation", "[bspline]") { + SECTION("1D linear") { + std::vector knots = {0, 0, 1, 1}; + Eigen::MatrixXd cp(2, 1); + cp << 0, 2; + BSpline bs(1, knots, cp); + + REQUIRE_THAT(bs.evaluate(0.0)(0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(bs.evaluate(0.5)(0), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(bs.evaluate(1.0)(0), WithinAbs(2.0, kNumericalAtol)); + } + + SECTION("2D curve endpoints") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(3, 2); + cp << 0, 0, 1, 1, 2, 0; + BSpline bs(2, knots, cp); + + auto p_min = bs.evaluate(bs.u_min()); + auto p_max = bs.evaluate(bs.u_max()); + REQUIRE_THAT(p_min(0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p_min(1), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p_max(0), WithinAbs(2.0, kRegularAtol)); + REQUIRE_THAT(p_max(1), WithinAbs(0.0, kRegularAtol)); + } + + SECTION("3D curve evaluation") { + std::vector knots = {0, 0, 1, 2, 2}; + Eigen::MatrixXd cp(3, 3); + cp << 0, 0, 0, 1, 1, 1, 2, 0, 2; + BSpline bs(1, knots, cp); + + auto pt = bs.evaluate(0.5); + REQUIRE(pt.size() == 3); + REQUIRE(pt.allFinite()); + } + + SECTION("Derivative order zero equals evaluate") { + std::vector knots = {0, 0, 0, 1, 2, 2, 2}; + Eigen::MatrixXd cp(4, 2); + cp << 0, 0, 1, 1, 2, 0, 3, 1; + BSpline bs(2, knots, cp); + + auto val = bs.evaluate(0.7); + auto deriv0 = bs.evaluate_derivative(0.7, 0); + for (int i = 0; i < val.size(); ++i) { + REQUIRE_THAT(val(i), WithinAbs(deriv0(i), kNumericalAtol)); + } + } + + SECTION("Derivative finite") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(3, 2); + cp << 0, 0, 1, 1, 2, 0; + BSpline bs(2, knots, cp); + + auto d1 = bs.evaluate_derivative(0.5, 1); + REQUIRE(d1.size() == 2); + REQUIRE(d1.allFinite()); + } + + SECTION("Derivative order exceeds degree") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(3, 1); + cp << 1, 2, 3; + BSpline bs(2, knots, cp); + + REQUIRE_THROWS_AS(bs.evaluate_derivative(0.5, 3), std::invalid_argument); + } + + SECTION("Finite difference vs exact derivative") { + std::vector knots = {0, 0, 0, 1, 2, 2, 2}; + Eigen::MatrixXd cp(4, 1); + cp << 1, 2, 3, 4; + BSpline bs(2, knots, cp); + + // Use a safe interior point (away from knots and boundaries) + double u = 0.5; + double h = 1e-5; + auto d_exact = bs.evaluate_derivative(u, 1); + auto val_plus = bs.evaluate(u + h); + auto val_minus = bs.evaluate(u - h); + double d_approx = (val_plus(0) - val_minus(0)) / (2.0 * h); + + REQUIRE_THAT(d_exact(0), WithinAbs(d_approx, 1e-3)); + } +} + +// --- Edge cases --- + +TEST_CASE("BSpline edge cases", "[bspline]") { + SECTION("Single control point degree 0") { + std::vector knots = {0, 1}; + Eigen::MatrixXd cp(1, 1); + cp << 5.0; + BSpline bs(0, knots, cp); + + REQUIRE_THAT(bs.evaluate(0.0)(0), WithinAbs(5.0, kNumericalAtol)); + REQUIRE_THAT(bs.evaluate(0.5)(0), WithinAbs(5.0, kNumericalAtol)); + REQUIRE_THAT(bs.evaluate(1.0)(0), WithinAbs(5.0, kNumericalAtol)); + } + + SECTION("Identical control points") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(3, 2); + cp << 1, 2, 1, 2, 1, 2; + BSpline bs(2, knots, cp); + + auto pt = bs.evaluate(0.5); + REQUIRE_THAT(pt(0), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(pt(1), WithinAbs(2.0, kNumericalAtol)); + } + + SECTION("Repeated internal knots") { + std::vector knots = {0, 0, 0, 0.5, 0.5, 1, 1, 1}; + Eigen::MatrixXd cp(5, 1); + cp << 1, 2, 3, 4, 5; + BSpline bs(2, knots, cp); + + auto pt = bs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } +} + +// --- Knot vector creation --- + +TEST_CASE("BSpline uniform knots", "[bspline]") { + SECTION("Basic creation") { + auto knots = BSpline::create_uniform_knots(2, 5); + REQUIRE(knots.size() == 8); + + // Non-decreasing + for (int i = 1; i < knots.size(); ++i) { + REQUIRE(knots[i] >= knots[i - 1]); + } + + // First and last clamped + for (int i = 0; i < 3; ++i) { + REQUIRE_THAT(knots[i], WithinAbs(0.0, kRegularAtol)); + } + for (int i = 5; i < 8; ++i) { + REQUIRE_THAT(knots[i], WithinAbs(1.0, kRegularAtol)); + } + } + + SECTION("Custom domain") { + auto knots = BSpline::create_uniform_knots(1, 3, -2.0, 5.0); + REQUIRE_THAT(knots[0], WithinAbs(-2.0, kRegularAtol)); + REQUIRE_THAT(knots[knots.size() - 1], WithinAbs(5.0, kRegularAtol)); + } + + SECTION("Validation") { + REQUIRE_THROWS_AS(BSpline::create_uniform_knots(-1, 5), std::invalid_argument); + REQUIRE_THROWS_AS(BSpline::create_uniform_knots(3, 3), std::invalid_argument); + } +} + +TEST_CASE("BSpline periodic knots", "[bspline]") { + SECTION("Basic creation") { + auto knots = BSpline::create_periodic_knots(2, 4); + REQUIRE(knots.size() == 7); + + // Non-decreasing + for (int i = 1; i < knots.size(); ++i) { + REQUIRE(knots[i] >= knots[i - 1]); + } + } + + SECTION("Validation") { + REQUIRE_THROWS_AS(BSpline::create_periodic_knots(-1, 3), std::invalid_argument); + REQUIRE_THROWS_AS(BSpline::create_periodic_knots(3, 2), std::invalid_argument); + } +} + +// --- Curve generation --- + +TEST_CASE("BSpline curve generation", "[bspline]") { + std::vector knots = {0, 0, 0, 1, 1, 1}; + Eigen::MatrixXd cp(3, 2); + cp << 0, 0, 1, 1, 2, 0; + BSpline bs(2, knots, cp); + + SECTION("Basic generation") { + auto [u_vals, pts] = bs.generate_curve_points(10); + REQUIRE(u_vals.size() == 10); + REQUIRE(pts.rows() == 10); + REQUIRE(pts.cols() == 2); + REQUIRE_THAT(u_vals[0], WithinAbs(bs.u_min(), kRegularAtol)); + REQUIRE_THAT(u_vals[9], WithinAbs(bs.u_max(), kRegularAtol)); + } + + SECTION("Different counts") { + for (int n : {5, 50, 100}) { + auto [u_vals, pts] = bs.generate_curve_points(n); + REQUIRE(u_vals.size() == n); + REQUIRE(pts.rows() == n); + } + } +} + +// --- Numerical stability --- + +TEST_CASE("BSpline numerical stability", "[bspline]") { + SECTION("Nearly coincident knots") { + std::vector knots = {0, 0, 0, 0.5, 0.5 + 1e-12, 1, 1, 1}; + Eigen::MatrixXd cp(5, 1); + cp << 1, 2, 3, 4, 5; + BSpline bs(2, knots, cp); + + auto pt = bs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } + + SECTION("High degree stability") { + auto knots = BSpline::create_uniform_knots(7, 10); + Eigen::MatrixXd cp(10, 2); + for (int i = 0; i < 10; ++i) { + cp(i, 0) = static_cast(i); + cp(i, 1) = std::sin(static_cast(i)); + } + BSpline bs(7, std::span(knots.data(), knots.size()), cp); + + int num_test = 20; + auto u_test = Eigen::VectorXd::LinSpaced(num_test, bs.u_min(), bs.u_max()); + for (int i = 0; i < num_test; ++i) { + auto pt = bs.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + } + } +} diff --git a/cpp/tests/test_bspline_variants.cpp b/cpp/tests/test_bspline_variants.cpp new file mode 100644 index 0000000..f31b5b0 --- /dev/null +++ b/cpp/tests/test_bspline_variants.cpp @@ -0,0 +1,558 @@ +#include +#include + +#include +#include +#include +#include +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::bspline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +// Helper: create quadratic 2D points [[i, i^2]] +static Eigen::MatrixXd make_quadratic_2d(int n) { + Eigen::MatrixXd pts(n, 2); + for (int i = 0; i < n; ++i) { + pts(i, 0) = static_cast(i); + pts(i, 1) = static_cast(i * i); + } + return pts; +} + +// Helper: create sine 2D points +static Eigen::MatrixXd make_sine_2d(int n, double x_max = 2.0 * M_PI) { + Eigen::MatrixXd pts(n, 2); + for (int i = 0; i < n; ++i) { + double x = x_max * i / (n - 1); + pts(i, 0) = x; + pts(i, 1) = std::sin(x); + } + return pts; +} + +// ===== CubicBSplineInterpolation ===== + +TEST_CASE("CubicBSplineInterpolation construction", "[cubic_bspline_interp]") { + SECTION("Basic construction") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9; + CubicBSplineInterpolation cbi(pts); + + REQUIRE(cbi.degree() == 3); + REQUIRE(cbi.dimension() == 2); + } + + SECTION("1D interpolation") { + Eigen::MatrixXd pts(5, 1); + pts << 0, 1, 4, 9, 16; + CubicBSplineInterpolation cbi(pts); + + REQUIRE(cbi.dimension() == 1); + auto pt = cbi.evaluate(0.5); + REQUIRE(pt.allFinite()); + } + + SECTION("3D interpolation") { + Eigen::MatrixXd pts(4, 3); + pts << 0, 0, 0, 1, 1, 1, 2, 4, 8, 3, 9, 27; + CubicBSplineInterpolation cbi(pts); + + REQUIRE(cbi.dimension() == 3); + auto pt = cbi.evaluate(0.5); + REQUIRE(pt.size() == 3); + REQUIRE(pt.allFinite()); + } +} + +TEST_CASE("CubicBSplineInterpolation endpoint accuracy", "[cubic_bspline_interp]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9; + CubicBSplineInterpolation cbi(pts); + + auto p_start = cbi.evaluate(cbi.u_min()); + auto p_end = cbi.evaluate(cbi.u_max()); + + REQUIRE_THAT(p_start(0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p_start(1), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p_end(0), WithinAbs(3.0, kRegularAtol)); + REQUIRE_THAT(p_end(1), WithinAbs(9.0, kRegularAtol)); +} + +TEST_CASE("CubicBSplineInterpolation parameterization methods", "[cubic_bspline_interp]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 0, 3, 1; + + for (auto method : + {Parameterization::EquallySpaced, Parameterization::ChordLength, + Parameterization::Centripetal}) { + CubicBSplineInterpolation cbi(pts, std::nullopt, std::nullopt, method); + + REQUIRE(cbi.degree() == 3); + auto pt = cbi.evaluate(0.5); + REQUIRE(pt.allFinite()); + } +} + +TEST_CASE("CubicBSplineInterpolation derivative constraints", "[cubic_bspline_interp]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9; + Eigen::VectorXd v0(2); + v0 << 1.0, 1.0; + Eigen::VectorXd vn(2); + vn << 1.0, 6.0; + + CubicBSplineInterpolation cbi(pts, v0, vn); + + REQUIRE(cbi.start_derivative().isApprox(v0)); + REQUIRE(cbi.end_derivative().isApprox(vn)); +} + +TEST_CASE("CubicBSplineInterpolation auto derivatives", "[cubic_bspline_interp]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9; + + SECTION("With auto") { + CubicBSplineInterpolation cbi(pts, std::nullopt, std::nullopt, + Parameterization::ChordLength, true); + REQUIRE(cbi.start_derivative().allFinite()); + REQUIRE(cbi.end_derivative().allFinite()); + } + + SECTION("Without auto") { + CubicBSplineInterpolation cbi(pts, std::nullopt, std::nullopt, + Parameterization::ChordLength, false); + REQUIRE(cbi.start_derivative().allFinite()); + } +} + +TEST_CASE("CubicBSplineInterpolation collinear points", "[cubic_bspline_interp]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 2, 3, 3; + CubicBSplineInterpolation cbi(pts); + + auto pt = cbi.evaluate(0.5); + REQUIRE(pt.allFinite()); +} + +// ===== BSplineInterpolator ===== + +TEST_CASE("BSplineInterpolator construction", "[bspline_interpolator]") { + SECTION("Basic degree 3") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9, 4, 16; + BSplineInterpolator bsi(3, pts); + + REQUIRE(bsi.degree() == 3); + auto pt = bsi.evaluate(0.5); + REQUIRE(pt.size() == 2); + REQUIRE(pt.allFinite()); + } + + SECTION("With times") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9, 4, 16; + Eigen::VectorXd times(5); + times << 0, 1, 2, 3, 4; + BSplineInterpolator bsi(3, pts, times); + + REQUIRE(bsi.degree() == 3); + } +} + +TEST_CASE("BSplineInterpolator degree validation", "[bspline_interpolator]") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9, 4, 16; + + SECTION("Invalid degree") { + REQUIRE_THROWS_AS(BSplineInterpolator(2, pts), std::invalid_argument); + } + + SECTION("Too few points") { + Eigen::MatrixXd few_pts(3, 2); + few_pts << 0, 0, 1, 1, 2, 4; + REQUIRE_THROWS_AS(BSplineInterpolator(5, few_pts), std::invalid_argument); + } +} + +TEST_CASE("BSplineInterpolator end conditions", "[bspline_interpolator]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 1, 2, 0, 3, 1; + BSplineInterpolator bsi(3, pts); + + auto p0 = bsi.evaluate(bsi.u_min()); + auto pn = bsi.evaluate(bsi.u_max()); + REQUIRE_THAT(p0(0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(p0(1), WithinAbs(0.0, kNumericalAtol)); +} + +TEST_CASE("BSplineInterpolator boundary conditions", "[bspline_interpolator]") { + Eigen::MatrixXd pts = make_quadratic_2d(10); + Eigen::VectorXd v0(2); + v0 << 1.0, 2.0; + Eigen::VectorXd vn(2); + vn << 1.0, 2.0; + + BSplineInterpolator bsi(3, pts, std::nullopt, v0, vn); + + auto pt = bsi.evaluate(0.5); + REQUIRE(pt.allFinite()); +} + +TEST_CASE("BSplineInterpolator cyclic", "[bspline_interpolator]") { + Eigen::MatrixXd pts(4, 2); + pts << 0, 0, 1, 0, 1, 1, 0, 1; + BSplineInterpolator bsi(3, pts, std::nullopt, std::nullopt, std::nullopt, + std::nullopt, std::nullopt, true); + + auto u_test = Eigen::VectorXd::LinSpaced(5, bsi.u_min(), bsi.u_max()); + for (int i = 0; i < 5; ++i) { + auto pt = bsi.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + } +} + +TEST_CASE("BSplineInterpolator 1D points", "[bspline_interpolator]") { + Eigen::MatrixXd pts(5, 1); + pts << 0, 1, 4, 9, 16; + BSplineInterpolator bsi(3, pts); + + auto pt = bsi.evaluate(0.5); + REQUIRE(pt.size() == 1); + REQUIRE(pt.allFinite()); +} + +TEST_CASE("BSplineInterpolator different degrees", "[bspline_interpolator]") { + // Use enough points for all degrees (degree 5 needs at least 6 points, + // but rank issues require more for boundary conditions) + Eigen::MatrixXd pts = make_quadratic_2d(20); + + for (int deg : {3, 4, 5}) { + BSplineInterpolator bsi(deg, pts); + REQUIRE(bsi.degree() == deg); + + auto u_test = Eigen::VectorXd::LinSpaced(15, bsi.u_min(), bsi.u_max()); + for (int i = 0; i < 15; ++i) { + auto pt = bsi.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + } + } +} + +// ===== ApproximationBSpline ===== + +TEST_CASE("ApproximationBSpline construction", "[approx_bspline]") { + SECTION("Basic construction") { + Eigen::MatrixXd pts(7, 2); + pts << 0, 0, 0.5, 0.25, 1, 1, 1.5, 2.25, 2, 4, 2.5, 6.25, 3, 9; + ApproximationBSpline abs(pts, 5, 3); + + REQUIRE(abs.degree() == 3); + } + + SECTION("Sine approximation") { + auto pts = make_sine_2d(15); + ApproximationBSpline abs(pts, 8, 3); + + auto u_test = Eigen::VectorXd::LinSpaced(20, abs.u_min(), abs.u_max()); + for (int i = 0; i < 20; ++i) { + auto pt = abs.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + REQUIRE(pt.size() == 2); + } + } +} + +TEST_CASE("ApproximationBSpline validation", "[approx_bspline]") { + auto pts = make_quadratic_2d(10); + + SECTION("Degree too low") { + REQUIRE_THROWS_AS(ApproximationBSpline(pts, 5, 0), std::invalid_argument); + } + + SECTION("Too few control points") { + REQUIRE_THROWS_AS(ApproximationBSpline(pts, 2, 3), std::invalid_argument); + } + + SECTION("More control points than data") { + Eigen::MatrixXd few(3, 2); + few << 0, 0, 1, 1, 2, 4; + REQUIRE_THROWS_AS(ApproximationBSpline(few, 5, 3), std::invalid_argument); + } +} + +TEST_CASE("ApproximationBSpline parameterization methods", "[approx_bspline]") { + auto pts = make_quadratic_2d(10); + + for (auto method : + {Parameterization::EquallySpaced, Parameterization::ChordLength, + Parameterization::Centripetal}) { + ApproximationBSpline abs(pts, 6, 3, std::nullopt, method); + auto pt = abs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } +} + +TEST_CASE("ApproximationBSpline error calculation", "[approx_bspline]") { + auto pts = make_sine_2d(25); + ApproximationBSpline abs(pts, 12, 3); + + double error = abs.calculate_approximation_error(); + REQUIRE(std::isfinite(error)); + REQUIRE(error >= 0.0); +} + +TEST_CASE("ApproximationBSpline original data storage", "[approx_bspline]") { + auto pts = make_quadratic_2d(10); + ApproximationBSpline abs(pts, 6, 3); + + REQUIRE(abs.original_points().rows() == 10); + REQUIRE(abs.original_parameters().size() == 10); + + // Parameters should be in [0, 1] + for (int i = 0; i < abs.original_parameters().size(); ++i) { + REQUIRE(abs.original_parameters()[i] >= 0.0); + REQUIRE(abs.original_parameters()[i] <= 1.0); + } +} + +TEST_CASE("ApproximationBSpline knot vector properties", "[approx_bspline]") { + auto pts = make_quadratic_2d(15); + ApproximationBSpline abs(pts, 8, 3); + + // n_knots = n_control + degree + 1 + REQUIRE(abs.knots().size() == abs.n_control_points() + abs.degree() + 1); + + // Non-decreasing + for (int i = 1; i < abs.knots().size(); ++i) { + REQUIRE(abs.knots()[i] >= abs.knots()[i - 1]); + } +} + +TEST_CASE("ApproximationBSpline different degrees", "[approx_bspline]") { + auto pts = make_quadratic_2d(12); + + for (int deg : {1, 2, 3, 4}) { + int n_ctrl = deg + 2; + if (n_ctrl >= static_cast(pts.rows())) continue; + ApproximationBSpline abs(pts, n_ctrl, deg); + auto pt = abs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } +} + +// ===== SmoothingCubicBSpline ===== + +TEST_CASE("SmoothingCubicBSpline construction", "[smoothing_bspline]") { + SECTION("Basic construction") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9, 4, 16; + SmoothingCubicBSpline sbs(pts); + + REQUIRE(sbs.degree() == 3); + } + + SECTION("With custom params") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 4, 3, 9, 4, 16; + BSplineParams params; + params.mu = 0.7; + params.method = Parameterization::Centripetal; + SmoothingCubicBSpline sbs(pts, params); + + REQUIRE(sbs.degree() == 3); + } +} + +TEST_CASE("SmoothingCubicBSpline smoothing parameters", "[smoothing_bspline]") { + auto pts = make_sine_2d(20); + + for (double mu : {0.1, 0.5, 0.9}) { + BSplineParams params; + params.mu = mu; + params.method = Parameterization::ChordLength; + SmoothingCubicBSpline sbs(pts, params); + + REQUIRE_THAT(sbs.mu(), WithinAbs(mu, kRegularAtol)); + } +} + +TEST_CASE("SmoothingCubicBSpline smoothing effect", "[smoothing_bspline]") { + // Noisy data + Eigen::MatrixXd pts(15, 2); + for (int i = 0; i < 15; ++i) { + double x = 4.0 * i / 14.0; + pts(i, 0) = x; + pts(i, 1) = x * x + 0.1 * std::sin(10.0 * x); // Quadratic + noise + } + + BSplineParams params; + params.mu = 0.5; + SmoothingCubicBSpline sbs(pts, params); + + auto u_test = Eigen::VectorXd::LinSpaced(10, sbs.u_min(), sbs.u_max()); + for (int i = 0; i < 10; ++i) { + auto pt = sbs.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + REQUIRE(pt.size() == 2); + } +} + +TEST_CASE("SmoothingCubicBSpline parameterization methods", "[smoothing_bspline]") { + auto pts = make_quadratic_2d(15); + + for (auto method : + {Parameterization::EquallySpaced, Parameterization::ChordLength, + Parameterization::Centripetal}) { + BSplineParams params; + params.method = method; + SmoothingCubicBSpline sbs(pts, params); + + auto u_test = Eigen::VectorXd::LinSpaced(10, sbs.u_min(), sbs.u_max()); + for (int i = 0; i < 10; ++i) { + auto pt = sbs.evaluate(u_test[i]); + REQUIRE(pt.allFinite()); + } + } +} + +TEST_CASE("SmoothingCubicBSpline endpoint enforcement", "[smoothing_bspline]") { + auto pts = make_quadratic_2d(10); + + SECTION("Without enforcement") { + SmoothingCubicBSpline sbs(pts); + auto pt = sbs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } + + SECTION("With enforcement") { + BSplineParams params; + params.enforce_endpoints = true; + SmoothingCubicBSpline sbs(pts, params); + auto pt = sbs.evaluate(0.5); + REQUIRE(pt.allFinite()); + } +} + +TEST_CASE("SmoothingCubicBSpline error calculation", "[smoothing_bspline]") { + auto pts = make_sine_2d(20); + SmoothingCubicBSpline sbs(pts); + + auto errors = sbs.calculate_approximation_error(); + REQUIRE(errors.size() == 20); + REQUIRE(errors.allFinite()); + + double total = sbs.calculate_total_error(); + REQUIRE(std::isfinite(total)); + REQUIRE(total >= 0.0); +} + +TEST_CASE("SmoothingCubicBSpline mu effects differ", "[smoothing_bspline]") { + // Use noisy data to make smoothing effects more pronounced + Eigen::MatrixXd pts(25, 2); + for (int i = 0; i < 25; ++i) { + double x = 2.0 * M_PI * i / 24.0; + pts(i, 0) = x; + pts(i, 1) = std::sin(x) + 0.3 * std::sin(10.0 * x); // Signal + noise + } + + BSplineParams params_tight; + params_tight.mu = 0.99; + SmoothingCubicBSpline sbs_tight(pts, params_tight); + + BSplineParams params_smooth; + params_smooth.mu = 0.01; + SmoothingCubicBSpline sbs_smooth(pts, params_smooth); + + // Check multiple evaluation points โ€” at least one should differ + double max_diff = 0.0; + auto u_test = Eigen::VectorXd::LinSpaced(10, sbs_tight.u_min(), sbs_tight.u_max()); + for (int i = 0; i < 10; ++i) { + auto pt_t = sbs_tight.evaluate(u_test[i]); + auto pt_s = sbs_smooth.evaluate(u_test[i]); + max_diff = std::max(max_diff, (pt_t - pt_s).norm()); + } + REQUIRE(max_diff > 1e-3); +} + +TEST_CASE("SmoothingCubicBSpline constant data", "[smoothing_bspline]") { + Eigen::MatrixXd pts(8, 2); + for (int i = 0; i < 8; ++i) { + pts(i, 0) = static_cast(i); + pts(i, 1) = 5.0; + } + + SmoothingCubicBSpline sbs(pts); + auto pt = sbs.evaluate(0.5); + REQUIRE(pt.allFinite()); + REQUIRE_THAT(pt(1), WithinAbs(5.0, 0.5)); +} + +TEST_CASE("SmoothingCubicBSpline auto derivatives", "[smoothing_bspline]") { + // Circle-like points + Eigen::MatrixXd pts(12, 2); + for (int i = 0; i < 12; ++i) { + double angle = 2.0 * M_PI * i / 12.0; + pts(i, 0) = std::cos(angle); + pts(i, 1) = std::sin(angle); + } + + SECTION("With auto derivatives") { + BSplineParams params; + params.enforce_endpoints = true; + params.auto_derivatives = true; + SmoothingCubicBSpline sbs(pts, params); + auto pt = sbs.evaluate(0.3); + REQUIRE(pt.allFinite()); + } + + SECTION("Without auto derivatives") { + BSplineParams params; + params.enforce_endpoints = true; + params.auto_derivatives = false; + SmoothingCubicBSpline sbs(pts, params); + auto pt = sbs.evaluate(0.3); + REQUIRE(pt.allFinite()); + } +} + +// ===== Variant inheritance ===== + +TEST_CASE("BSpline variant inheritance", "[bspline_variants]") { + Eigen::MatrixXd pts(5, 2); + pts << 0, 0, 1, 1, 2, 0, 3, 1, 4, 0; + + auto extended_pts = make_quadratic_2d(8); + + SECTION("CubicBSplineInterpolation inherits BSpline") { + CubicBSplineInterpolation cbi(pts); + const BSpline& base = cbi; + REQUIRE(base.degree() == 3); + REQUIRE(base.evaluate(0.5).allFinite()); + } + + SECTION("BSplineInterpolator inherits BSpline") { + BSplineInterpolator bsi(3, pts); + const BSpline& base = bsi; + REQUIRE(base.degree() == 3); + REQUIRE(base.evaluate(0.5).allFinite()); + } + + SECTION("ApproximationBSpline inherits BSpline") { + ApproximationBSpline abs(extended_pts, 5, 3); + const BSpline& base = abs; + REQUIRE(base.degree() == 3); + REQUIRE(base.evaluate(0.5).allFinite()); + } + + SECTION("SmoothingCubicBSpline inherits BSpline") { + SmoothingCubicBSpline sbs(pts); + const BSpline& base = sbs; + REQUIRE(base.degree() == 3); + REQUIRE(base.evaluate(0.5).allFinite()); + } +} diff --git a/cpp/tests/test_concepts.cpp b/cpp/tests/test_concepts.cpp new file mode 100644 index 0000000..6a0d243 --- /dev/null +++ b/cpp/tests/test_concepts.cpp @@ -0,0 +1,67 @@ +#include + +#include +#include +#include +#include +#include +// Phase 2 +#include +#include +#include +#include +#include +// Phase 4 +#include +#include +#include +// Phase 5 +#include +#include + +using namespace interpolatecpp; +using namespace interpolatecpp::spline; + +// Compile-time concept conformance checks +static_assert(ScalarTrajectory, + "CubicSpline must satisfy ScalarTrajectory concept"); +static_assert(ScalarTrajectory, + "CubicSmoothingSpline must satisfy ScalarTrajectory concept"); +static_assert(ScalarTrajectory, + "CubicSplineWithAcceleration1 must satisfy ScalarTrajectory concept"); +static_assert(ScalarTrajectory, + "CubicSplineWithAcceleration2 must satisfy ScalarTrajectory concept"); + +// Phase 2: B-spline family +static_assert(CurveEvaluator, + "BSpline must satisfy CurveEvaluator concept"); +static_assert(CurveEvaluator, + "CubicBSplineInterpolation must satisfy CurveEvaluator concept"); +static_assert(CurveEvaluator, + "BSplineInterpolator must satisfy CurveEvaluator concept"); +static_assert(CurveEvaluator, + "ApproximationBSpline must satisfy CurveEvaluator concept"); +static_assert(CurveEvaluator, + "SmoothingCubicBSpline must satisfy CurveEvaluator concept"); + +// Phase 4: Quaternion trajectory +static_assert(QuaternionTrajectory, + "SquadC2 must satisfy QuaternionTrajectory concept"); +static_assert(QuaternionTrajectory, + "LogQuaternionInterpolation must satisfy QuaternionTrajectory concept"); +static_assert(QuaternionTrajectory, + "QuaternionSpline must satisfy QuaternionTrajectory concept"); +// Note: ModifiedLogQuaternionInterpolation uses 4D velocity/acceleration +// so it deliberately does NOT satisfy QuaternionTrajectory (3D). + +// Phase 5: Path primitives +static_assert(GeometricPath, + "LinearPath must satisfy GeometricPath concept"); +static_assert(GeometricPath, + "CircularPath must satisfy GeometricPath concept"); + +TEST_CASE("Concept conformance compiles", "[concepts]") { + // The static_asserts above are the real test โ€” this just ensures + // the test file is linked and the assertions are evaluated. + REQUIRE(true); +} diff --git a/cpp/tests/test_cubic_smoothing_spline.cpp b/cpp/tests/test_cubic_smoothing_spline.cpp new file mode 100644 index 0000000..f987254 --- /dev/null +++ b/cpp/tests/test_cubic_smoothing_spline.cpp @@ -0,0 +1,176 @@ +#include +#include + +#include + +#include "test_data.hpp" + +#include +#include + +using namespace interpolatecpp::spline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("CubicSmoothingSpline construction", "[smoothing_spline]") { + SECTION("Basic construction") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector q = {0.0, 1.0, 0.5, 1.5, 1.0}; + CubicSmoothingSpline spline(t, q); + + REQUIRE(spline.n_points() == 5); + REQUIRE(spline.mu() == 0.5); + REQUIRE(spline.coefficients().rows() == 4); + REQUIRE(spline.coefficients().cols() == 4); + } + + SECTION("Input validation - mismatched lengths") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSmoothingSpline(t, q), std::invalid_argument); + } + + SECTION("Input validation - insufficient points") { + std::vector t = {0.0}; + std::vector q = {0.0}; + REQUIRE_THROWS_AS(CubicSmoothingSpline(t, q), std::invalid_argument); + } + + SECTION("Input validation - non-increasing time") { + std::vector t = {0.0, 2.0, 1.0, 3.0}; + std::vector q = {0.0, 1.0, 0.5, 1.5}; + REQUIRE_THROWS_AS(CubicSmoothingSpline(t, q), std::invalid_argument); + } + + SECTION("Input validation - invalid mu (zero)") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0, 0.5}; + REQUIRE_THROWS_AS(CubicSmoothingSpline(t, q, 0.0), std::invalid_argument); + } + + SECTION("Input validation - invalid mu (> 1)") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0, 0.5}; + REQUIRE_THROWS_AS(CubicSmoothingSpline(t, q, 1.1), std::invalid_argument); + } + + SECTION("Input validation - mismatched weights") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0, 0.5}; + std::vector w = {1.0, 1.0}; // wrong length + REQUIRE_THROWS_AS( + CubicSmoothingSpline(t, q, 0.5, std::span(w)), + std::invalid_argument); + } +} + +TEST_CASE("CubicSmoothingSpline exact interpolation (mu=1)", "[smoothing_spline]") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector q = {0.0, 1.0, 0.5, 1.5, 1.0}; + CubicSmoothingSpline spline(t, q, 1.0); + + SECTION("Passes through waypoints") { + for (int i = 0; i < 5; ++i) { + REQUIRE_THAT(spline.evaluate(t[static_cast(i)]), + WithinAbs(q[static_cast(i)], kNumericalAtol)); + } + } + + SECTION("Approximated points equal original") { + for (int i = 0; i < 5; ++i) { + REQUIRE_THAT(spline.s_points()(i), WithinAbs(spline.q_points()(i), kRegularAtol)); + } + } +} + +TEST_CASE("CubicSmoothingSpline smoothing effect", "[smoothing_spline]") { + // Noisy sine data + std::vector t = {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0}; + std::vector q = {0.0, 0.55, 0.84, 0.92, 0.91, 0.45, 0.14, -0.25, -0.76}; + + SECTION("mu=1 gives exact interpolation") { + CubicSmoothingSpline spline(t, q, 1.0); + double max_error = (spline.q_points() - spline.s_points()).cwiseAbs().maxCoeff(); + REQUIRE_THAT(max_error, WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Small mu gives heavy smoothing") { + CubicSmoothingSpline spline(t, q, 0.01); + // Smoothed points should differ from original + double max_error = (spline.q_points() - spline.s_points()).cwiseAbs().maxCoeff(); + REQUIRE(max_error > 0.01); + } + + SECTION("Smoothing reduces acceleration magnitude") { + CubicSmoothingSpline interp(t, q, 1.0); + CubicSmoothingSpline smooth(t, q, 0.1); + + // Evaluate accelerations at several points + double max_acc_interp = 0.0; + double max_acc_smooth = 0.0; + for (double ti = 0.0; ti <= 4.0; ti += 0.1) { + max_acc_interp = std::max(max_acc_interp, std::abs(interp.evaluate_acceleration(ti))); + max_acc_smooth = std::max(max_acc_smooth, std::abs(smooth.evaluate_acceleration(ti))); + } + REQUIRE(max_acc_smooth < max_acc_interp); + } +} + +TEST_CASE("CubicSmoothingSpline with weights", "[smoothing_spline]") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector q = {0.0, 1.0, 0.5, 1.5, 1.0}; + std::vector high_endpoint_weights = {10.0, 1.0, 1.0, 1.0, 10.0}; + + SECTION("Higher weights at endpoints enforce closer fit") { + CubicSmoothingSpline weighted(t, q, 0.1, + std::span(high_endpoint_weights)); + CubicSmoothingSpline uniform(t, q, 0.1); + + // Endpoint errors should be smaller with higher weights + double weighted_endpoint_err = + std::abs(weighted.s_points()(0) - q[0]) + std::abs(weighted.s_points()(4) - q[4]); + double uniform_endpoint_err = + std::abs(uniform.s_points()(0) - q[0]) + std::abs(uniform.s_points()(4) - q[4]); + + REQUIRE(weighted_endpoint_err <= uniform_endpoint_err + kNumericalAtol); + } +} + +TEST_CASE("CubicSmoothingSpline evaluation", "[smoothing_spline]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSmoothingSpline spline(t, q, 0.5); + + SECTION("All outputs are finite") { + for (double ti = 0.0; ti <= 3.0; ti += 0.25) { + REQUIRE(std::isfinite(spline.evaluate(ti))); + REQUIRE(std::isfinite(spline.evaluate_velocity(ti))); + REQUIRE(std::isfinite(spline.evaluate_acceleration(ti))); + } + } + + SECTION("Vectorized matches scalar") { + Eigen::VectorXd tv = Eigen::VectorXd::LinSpaced(15, 0.0, 3.0); + Eigen::VectorXd result = spline.evaluate(tv); + for (Eigen::Index i = 0; i < tv.size(); ++i) { + REQUIRE_THAT(result(i), WithinAbs(spline.evaluate(tv(i)), kRegularAtol)); + } + } +} + +TEST_CASE("CubicSmoothingSpline edge cases", "[smoothing_spline]") { + SECTION("Two points") { + std::vector t = {0.0, 1.0}; + std::vector q = {0.0, 1.0}; + CubicSmoothingSpline spline(t, q, 1.0); + REQUIRE(std::isfinite(spline.evaluate(0.5))); + } + + SECTION("Constant data") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {3.0, 3.0, 3.0, 3.0}; + CubicSmoothingSpline spline(t, q, 1.0); + + REQUIRE_THAT(spline.evaluate(1.5), WithinAbs(3.0, kNumericalAtol)); + } +} diff --git a/cpp/tests/test_cubic_spline.cpp b/cpp/tests/test_cubic_spline.cpp new file mode 100644 index 0000000..48fc8e4 --- /dev/null +++ b/cpp/tests/test_cubic_spline.cpp @@ -0,0 +1,266 @@ +#include +#include + +#include + +#include "test_data.hpp" + +#include +#include + +using namespace interpolatecpp::spline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; +using Catch::Matchers::WithinRel; + +TEST_CASE("CubicSpline construction", "[cubic_spline]") { + SECTION("Basic construction") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + + REQUIRE(spline.n_segments() == 3); + REQUIRE(spline.t_points().size() == 4); + REQUIRE(spline.q_points().size() == 4); + REQUIRE(spline.velocities().size() == 4); + REQUIRE(spline.coefficients().rows() == 3); + REQUIRE(spline.coefficients().cols() == 4); + } + + SECTION("With boundary velocities") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q, 1.0, -1.0); + + REQUIRE_THAT(spline.velocities()(0), WithinAbs(1.0, kRegularAtol)); + REQUIRE_THAT(spline.velocities()(3), WithinAbs(-1.0, kRegularAtol)); + } + + SECTION("Mismatched lengths throw") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSpline(t, q), std::invalid_argument); + } + + SECTION("Non-increasing time throws") { + std::vector t = {0.0, 2.0, 1.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSpline(t, q), std::invalid_argument); + } + + SECTION("Repeated time throws") { + std::vector t = {0.0, 1.0, 1.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSpline(t, q), std::invalid_argument); + } + + SECTION("Single segment (2 points)") { + std::vector t = {0.0, 1.0}; + std::vector q = {0.0, 1.0}; + CubicSpline spline(t, q); + + REQUIRE(spline.n_segments() == 1); + REQUIRE_THAT(spline.evaluate(0.0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(spline.evaluate(1.0), WithinAbs(1.0, kRegularAtol)); + } + + SECTION("Two segments (3 points)") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0, 0.0}; + CubicSpline spline(t, q); + + REQUIRE(spline.n_segments() == 2); + } +} + +TEST_CASE("CubicSpline mathematical accuracy", "[cubic_spline]") { + SECTION("Linear function is exact") { + // y = 2x with v0=2, vn=2 + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 2.0, 4.0, 6.0}; + CubicSpline spline(t, q, 2.0, 2.0); + + for (double ti = 0.0; ti <= 3.0; ti += 0.25) { + REQUIRE_THAT(spline.evaluate(ti), WithinAbs(2.0 * ti, kRegularAtol)); + REQUIRE_THAT(spline.evaluate_velocity(ti), WithinAbs(2.0, kNumericalAtol)); + REQUIRE_THAT(spline.evaluate_acceleration(ti), WithinAbs(0.0, kNumericalAtol)); + } + } + + SECTION("Quadratic function") { + // y = x^2, v0=0, vn=6 + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 4.0, 9.0}; + CubicSpline spline(t, q, 0.0, 6.0); + + for (double ti = 0.0; ti <= 3.0; ti += 0.5) { + REQUIRE_THAT(spline.evaluate(ti), WithinAbs(ti * ti, kNumericalAtol)); + } + } + + SECTION("Waypoint interpolation exactness") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + + // Must pass exactly through waypoints + for (int i = 0; i < 4; ++i) { + REQUIRE_THAT(spline.evaluate(t[static_cast(i)]), + WithinAbs(q[static_cast(i)], kRegularAtol)); + } + } + + SECTION("Symmetric trajectory") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector q = {0.0, 1.0, 2.0, 1.0, 0.0}; + CubicSpline spline(t, q); + + // Symmetric about midpoint + REQUIRE_THAT(spline.evaluate(1.0), WithinAbs(spline.evaluate(3.0), kNumericalAtol)); + REQUIRE_THAT(spline.evaluate(0.5), WithinAbs(spline.evaluate(3.5), kNumericalAtol)); + } +} + +TEST_CASE("CubicSpline C2 continuity", "[cubic_spline]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + double eps = 1e-8; + + SECTION("C0 continuity (position)") { + for (int i = 1; i < 3; ++i) { + double ti = t[static_cast(i)]; + double left = spline.evaluate(ti - eps); + double right = spline.evaluate(ti + eps); + REQUIRE_THAT(left, WithinAbs(right, kNumericalAtol)); + } + } + + SECTION("C1 continuity (velocity)") { + for (int i = 1; i < 3; ++i) { + double ti = t[static_cast(i)]; + double left = spline.evaluate_velocity(ti - eps); + double right = spline.evaluate_velocity(ti + eps); + REQUIRE_THAT(left, WithinAbs(right, kNumericalAtol)); + } + } + + SECTION("C2 continuity (acceleration)") { + for (int i = 1; i < 3; ++i) { + double ti = t[static_cast(i)]; + double left = spline.evaluate_acceleration(ti - eps); + double right = spline.evaluate_acceleration(ti + eps); + REQUIRE_THAT(left, WithinAbs(right, kNumericalAtol)); + } + } +} + +TEST_CASE("CubicSpline evaluation methods", "[cubic_spline]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + + SECTION("Vectorized evaluation matches scalar") { + Eigen::VectorXd tv = Eigen::VectorXd::LinSpaced(20, 0.0, 3.0); + Eigen::VectorXd result = spline.evaluate(tv); + + for (Eigen::Index i = 0; i < tv.size(); ++i) { + REQUIRE_THAT(result(i), WithinAbs(spline.evaluate(tv(i)), kRegularAtol)); + } + } + + SECTION("Vectorized velocity matches scalar") { + Eigen::VectorXd tv = Eigen::VectorXd::LinSpaced(20, 0.0, 3.0); + Eigen::VectorXd result = spline.evaluate_velocity(tv); + + for (Eigen::Index i = 0; i < tv.size(); ++i) { + REQUIRE_THAT(result(i), WithinAbs(spline.evaluate_velocity(tv(i)), kRegularAtol)); + } + } + + SECTION("Boundary extrapolation (clamp)") { + // Before start: clamps to segment 0, tau=0 + double before = spline.evaluate(-1.0); + double at_start = spline.evaluate(0.0); + REQUIRE_THAT(before, WithinAbs(at_start, kRegularAtol)); + + // After end: clamps to last segment end + double after = spline.evaluate(4.0); + double at_end = spline.evaluate(3.0); + REQUIRE_THAT(after, WithinAbs(at_end, kRegularAtol)); + } + + SECTION("Evaluation consistency via finite differences") { + double t_test = 1.5; + double h = 1e-7; + double pos = spline.evaluate(t_test); + double pos_h = spline.evaluate(t_test + h); + double vel = spline.evaluate_velocity(t_test); + double numerical_vel = (pos_h - pos) / h; + + REQUIRE_THAT(vel, WithinAbs(numerical_vel, kLooseAtol)); + } +} + +TEST_CASE("CubicSpline edge cases", "[cubic_spline]") { + SECTION("Identical positions") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {5.0, 5.0, 5.0, 5.0}; + CubicSpline spline(t, q); + + REQUIRE_THAT(spline.evaluate(1.5), WithinAbs(5.0, kNumericalAtol)); + REQUIRE_THAT(spline.evaluate_velocity(1.5), WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Negative time values") { + std::vector t = {-2.0, -1.0, 0.0, 1.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + + REQUIRE(spline.n_segments() == 3); + REQUIRE_THAT(spline.evaluate(-2.0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(spline.evaluate(1.0), WithinAbs(1.0, kRegularAtol)); + } + + SECTION("Large time intervals") { + std::vector t = {0.0, 100.0, 200.0, 300.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q); + + REQUIRE(std::isfinite(spline.evaluate(150.0))); + REQUIRE(std::isfinite(spline.evaluate_velocity(150.0))); + } + + SECTION("Zero boundary velocities") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSpline spline(t, q, 0.0, 0.0); + + REQUIRE_THAT(spline.evaluate_velocity(0.0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(spline.evaluate_velocity(3.0), WithinAbs(0.0, kRegularAtol)); + } +} + +TEST_CASE("CubicSpline numerical stability", "[cubic_spline]") { + SECTION("Convergence with increasing points") { + // Interpolate sin(x) on [0, pi] โ€” error should decrease with more points + auto make_spline = [](int n) { + std::vector t(static_cast(n)); + std::vector q(static_cast(n)); + for (int i = 0; i < n; ++i) { + t[static_cast(i)] = M_PI * i / (n - 1); + q[static_cast(i)] = std::sin(t[static_cast(i)]); + } + return CubicSpline(t, q, std::cos(0.0), std::cos(M_PI)); + }; + + auto spline_10 = make_spline(10); + auto spline_50 = make_spline(50); + + // Evaluate at midpoint + double error_10 = std::abs(spline_10.evaluate(M_PI / 2.0) - 1.0); + double error_50 = std::abs(spline_50.evaluate(M_PI / 2.0) - 1.0); + + REQUIRE(error_50 < error_10); + } +} diff --git a/cpp/tests/test_cubic_spline_with_acc1.cpp b/cpp/tests/test_cubic_spline_with_acc1.cpp new file mode 100644 index 0000000..7e8572e --- /dev/null +++ b/cpp/tests/test_cubic_spline_with_acc1.cpp @@ -0,0 +1,125 @@ +#include +#include + +#include + +#include "test_data.hpp" + +#include +#include + +using namespace interpolatecpp::spline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("CubicSplineWithAcceleration1 construction", "[acc1]") { + SECTION("Basic construction") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSplineWithAcceleration1 spline(t, q); + + // Original 4 points + 2 extra = 6 total + REQUIRE(spline.n_points() == 6); + REQUIRE(spline.n_orig() == 4); + REQUIRE(spline.original_indices().size() == 4); + } + + SECTION("With acceleration constraints") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSplineWithAcceleration1 spline(t, q, 0.0, 0.0, 2.0, 2.0); + + REQUIRE_THAT(spline.omega()(0), WithinAbs(2.0, kRegularAtol)); + REQUIRE_THAT(spline.omega()(spline.n_points() - 1), WithinAbs(2.0, kRegularAtol)); + } + + SECTION("Input validation - mismatched lengths") { + std::vector t = {0.0, 1.0, 2.0}; + std::vector q = {0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSplineWithAcceleration1(t, q), std::invalid_argument); + } + + SECTION("Input validation - non-increasing time") { + std::vector t = {0.0, 2.0, 1.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + REQUIRE_THROWS_AS(CubicSplineWithAcceleration1(t, q), std::invalid_argument); + } + + SECTION("Input validation - insufficient points") { + std::vector t = {0.0}; + std::vector q = {0.0}; + REQUIRE_THROWS_AS(CubicSplineWithAcceleration1(t, q), std::invalid_argument); + } +} + +TEST_CASE("CubicSplineWithAcceleration1 boundary conditions", "[acc1]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + + SECTION("Boundary velocity enforcement") { + CubicSplineWithAcceleration1 spline(t, q, 1.0, -1.0, 0.0, 0.0); + REQUIRE_THAT(spline.evaluate_velocity(0.0), WithinAbs(1.0, 1.0)); + } + + SECTION("Boundary acceleration enforcement") { + CubicSplineWithAcceleration1 spline(t, q, 0.0, 0.0, 2.0, -2.0); + REQUIRE_THAT(spline.evaluate_acceleration(0.0), WithinAbs(2.0, 2.0)); + } +} + +TEST_CASE("CubicSplineWithAcceleration1 evaluation", "[acc1]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSplineWithAcceleration1 spline(t, q, 0.0, 0.0, 0.0, 0.0); + + SECTION("All outputs are finite") { + for (double ti = 0.0; ti <= 3.0; ti += 0.25) { + REQUIRE(std::isfinite(spline.evaluate(ti))); + REQUIRE(std::isfinite(spline.evaluate_velocity(ti))); + REQUIRE(std::isfinite(spline.evaluate_acceleration(ti))); + } + } + + SECTION("Vectorized matches scalar") { + Eigen::VectorXd tv = Eigen::VectorXd::LinSpaced(20, 0.0, 3.0); + Eigen::VectorXd result = spline.evaluate(tv); + for (Eigen::Index i = 0; i < tv.size(); ++i) { + REQUIRE_THAT(result(i), WithinAbs(spline.evaluate(tv(i)), kRegularAtol)); + } + } + + SECTION("Passes through original endpoints") { + REQUIRE_THAT(spline.evaluate(0.0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(spline.evaluate(3.0), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Extra points computation") { + // n_orig + 2 = total points + REQUIRE(spline.t_points().size() == spline.q_points().size()); + REQUIRE(spline.t_points().size() == spline.n_orig() + 2); + } +} + +TEST_CASE("CubicSplineWithAcceleration1 derivative evaluation", "[acc1]") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector q = {0.0, 1.0, 4.0, 9.0, 16.0}; + CubicSplineWithAcceleration1 spline(t, q, 0.0, 8.0, 2.0, 2.0); + + SECTION("Finite differences match velocity") { + // Use mid-segment point to avoid straddling segment boundaries + double t_test = 1.5; + double h = 1e-7; + double numerical_vel = (spline.evaluate(t_test + h) - spline.evaluate(t_test - h)) / (2.0 * h); + double vel = spline.evaluate_velocity(t_test); + REQUIRE_THAT(vel, WithinAbs(numerical_vel, kLooseAtol)); + } + + SECTION("Finite differences match acceleration") { + double t_test = 1.5; + double h = 1e-5; + double numerical_acc = (spline.evaluate_velocity(t_test + h) - + spline.evaluate_velocity(t_test - h)) / (2.0 * h); + double acc = spline.evaluate_acceleration(t_test); + REQUIRE_THAT(acc, WithinAbs(numerical_acc, kLooseAtol)); + } +} diff --git a/cpp/tests/test_cubic_spline_with_acc2.cpp b/cpp/tests/test_cubic_spline_with_acc2.cpp new file mode 100644 index 0000000..b1bae08 --- /dev/null +++ b/cpp/tests/test_cubic_spline_with_acc2.cpp @@ -0,0 +1,171 @@ +#include +#include + +#include + +#include "test_data.hpp" + +#include +#include + +using namespace interpolatecpp::spline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("CubicSplineWithAcceleration2 construction", "[acc2]") { + SECTION("Basic construction (no acceleration constraints)") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSplineWithAcceleration2 spline(t, q); + + REQUIRE(spline.n_segments() == 3); + REQUIRE_FALSE(spline.has_quintic_first()); + REQUIRE_FALSE(spline.has_quintic_last()); + } + + SECTION("Inherits from CubicSpline") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + CubicSplineWithAcceleration2 spline(t, q); + + // Should have all CubicSpline accessors + REQUIRE(spline.t_points().size() == 4); + REQUIRE(spline.q_points().size() == 4); + REQUIRE(spline.velocities().size() == 4); + } + + SECTION("With initial acceleration constraint") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + SplineParameters params{.a0 = 2.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE(spline.has_quintic_first()); + REQUIRE_FALSE(spline.has_quintic_last()); + } + + SECTION("With final acceleration constraint") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + SplineParameters params{.an = -1.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE_FALSE(spline.has_quintic_first()); + REQUIRE(spline.has_quintic_last()); + } + + SECTION("With both acceleration constraints") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + SplineParameters params{.a0 = 2.0, .an = -1.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE(spline.has_quintic_first()); + REQUIRE(spline.has_quintic_last()); + } +} + +TEST_CASE("CubicSplineWithAcceleration2 acceleration constraints", "[acc2]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + + SECTION("Initial acceleration is satisfied") { + SplineParameters params{.a0 = 2.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE_THAT(spline.evaluate_acceleration(0.0), WithinAbs(2.0, kNumericalAtol)); + } + + SECTION("Final acceleration is satisfied") { + SplineParameters params{.an = -3.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE_THAT(spline.evaluate_acceleration(3.0), WithinAbs(-3.0, kNumericalAtol)); + } + + SECTION("Both accelerations satisfied") { + SplineParameters params{.a0 = 2.0, .an = -3.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + REQUIRE_THAT(spline.evaluate_acceleration(0.0), WithinAbs(2.0, kNumericalAtol)); + REQUIRE_THAT(spline.evaluate_acceleration(3.0), WithinAbs(-3.0, kNumericalAtol)); + } +} + +TEST_CASE("CubicSplineWithAcceleration2 evaluation", "[acc2]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + SplineParameters params{.a0 = 1.0, .an = -1.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + SECTION("Passes through waypoints") { + for (int i = 0; i < 4; ++i) { + REQUIRE_THAT(spline.evaluate(t[static_cast(i)]), + WithinAbs(q[static_cast(i)], kNumericalAtol)); + } + } + + SECTION("All outputs are finite") { + for (double ti = 0.0; ti <= 3.0; ti += 0.25) { + REQUIRE(std::isfinite(spline.evaluate(ti))); + REQUIRE(std::isfinite(spline.evaluate_velocity(ti))); + REQUIRE(std::isfinite(spline.evaluate_acceleration(ti))); + } + } + + SECTION("Vectorized matches scalar") { + Eigen::VectorXd tv = Eigen::VectorXd::LinSpaced(20, 0.0, 3.0); + Eigen::VectorXd result = spline.evaluate(tv); + for (Eigen::Index i = 0; i < tv.size(); ++i) { + REQUIRE_THAT(result(i), WithinAbs(spline.evaluate(tv(i)), kRegularAtol)); + } + } + + SECTION("Boundary extrapolation") { + // Before start + double before = spline.evaluate(-0.5); + REQUIRE(std::isfinite(before)); + // After end + double after = spline.evaluate(3.5); + REQUIRE(std::isfinite(after)); + } +} + +TEST_CASE("CubicSplineWithAcceleration2 derivative evaluation", "[acc2]") { + std::vector t = {0.0, 1.0, 2.0, 3.0}; + std::vector q = {0.0, 1.0, 0.0, 1.0}; + SplineParameters params{.a0 = 1.0, .an = -1.0}; + CubicSplineWithAcceleration2 spline(t, q, params); + + SECTION("Finite differences match velocity") { + // Test in quintic first segment + double h = 1e-7; + double t_test = 0.5; + double numerical = (spline.evaluate(t_test + h) - spline.evaluate(t_test - h)) / (2.0 * h); + REQUIRE_THAT(spline.evaluate_velocity(t_test), WithinAbs(numerical, kLooseAtol)); + + // Test in cubic middle segment + t_test = 1.5; + numerical = (spline.evaluate(t_test + h) - spline.evaluate(t_test - h)) / (2.0 * h); + REQUIRE_THAT(spline.evaluate_velocity(t_test), WithinAbs(numerical, kLooseAtol)); + + // Test in quintic last segment + t_test = 2.5; + numerical = (spline.evaluate(t_test + h) - spline.evaluate(t_test - h)) / (2.0 * h); + REQUIRE_THAT(spline.evaluate_velocity(t_test), WithinAbs(numerical, kLooseAtol)); + } +} + +TEST_CASE("CubicSplineWithAcceleration2 error handling", "[acc2]") { + SECTION("Insufficient points") { + std::vector t = {0.0}; + std::vector q = {0.0}; + REQUIRE_THROWS_AS(CubicSplineWithAcceleration2(t, q), std::invalid_argument); + } + + SECTION("Non-monotonic time") { + std::vector t = {0.0, 2.0, 1.0}; + std::vector q = {0.0, 1.0, 0.0}; + REQUIRE_THROWS_AS(CubicSplineWithAcceleration2(t, q), std::invalid_argument); + } +} diff --git a/cpp/tests/test_double_s_trajectory.cpp b/cpp/tests/test_double_s_trajectory.cpp new file mode 100644 index 0000000..e02b86e --- /dev/null +++ b/cpp/tests/test_double_s_trajectory.cpp @@ -0,0 +1,157 @@ +#include +#include + +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::motion; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("DoubleSTrajectory construction", "[double_s]") { + SECTION("Basic construction") { + StateParams state{0.0, 10.0, 0.0, 0.0}; + TrajectoryBounds bounds(2.0, 1.0, 0.5); + DoubleSTrajectory traj(state, bounds); + + REQUIRE(traj.duration() > 0.0); + } + + SECTION("Various states") { + // Forward + StateParams s1{0.0, 10.0, 0.0, 0.0}; + TrajectoryBounds b(5.0, 2.0, 1.0); + DoubleSTrajectory t1(s1, b); + REQUIRE(t1.duration() > 0.0); + + // Backward + StateParams s2{10.0, 0.0, 0.0, 0.0}; + DoubleSTrajectory t2(s2, b); + REQUIRE(t2.duration() > 0.0); + } +} + +TEST_CASE("DoubleSTrajectory bounds validation", "[double_s]") { + SECTION("Zero bounds rejected") { + REQUIRE_THROWS_AS(TrajectoryBounds(0.0, 1.0, 0.5), std::invalid_argument); + } + + SECTION("Negative bounds convert to absolute") { + TrajectoryBounds b(-2.0, -1.0, -0.5); + REQUIRE(b.v_bound > 0); + REQUIRE(b.a_bound > 0); + REQUIRE(b.j_bound > 0); + } +} + +TEST_CASE("DoubleSTrajectory evaluation", "[double_s]") { + StateParams state{0.0, 10.0, 0.0, 0.0}; + TrajectoryBounds bounds(3.0, 2.0, 1.0); + DoubleSTrajectory traj(state, bounds); + + SECTION("Start and end") { + auto r0 = traj.evaluate(0.0); + auto rT = traj.evaluate(traj.duration()); + + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(rT.position, WithinAbs(10.0, kNumericalAtol)); + } + + SECTION("Midpoint finite") { + auto rm = traj.evaluate(traj.duration() / 2.0); + REQUIRE(std::isfinite(rm.position)); + REQUIRE(std::isfinite(rm.velocity)); + REQUIRE(std::isfinite(rm.acceleration)); + REQUIRE(std::isfinite(rm.jerk)); + } +} + +TEST_CASE("DoubleSTrajectory boundary conditions", "[double_s]") { + StateParams state{2.0, 8.0, 1.0, 0.5}; + TrajectoryBounds bounds(5.0, 3.0, 2.0); + DoubleSTrajectory traj(state, bounds); + + auto r0 = traj.evaluate(0.0); + auto rT = traj.evaluate(traj.duration()); + + REQUIRE_THAT(r0.position, WithinAbs(2.0, kNumericalAtol)); + REQUIRE_THAT(rT.position, WithinAbs(8.0, kNumericalAtol)); +} + +TEST_CASE("DoubleSTrajectory velocity bounds", "[double_s]") { + StateParams state{0.0, 20.0, 0.0, 0.0}; + TrajectoryBounds bounds(2.0, 1.0, 0.5); + DoubleSTrajectory traj(state, bounds); + + double T = traj.duration(); + for (int i = 0; i <= 100; ++i) { + double t = T * i / 100.0; + auto r = traj.evaluate(t); + REQUIRE(std::abs(r.velocity) <= bounds.v_bound + kNumericalAtol); + } +} + +TEST_CASE("DoubleSTrajectory phase durations", "[double_s]") { + StateParams state{0.0, 10.0, 0.0, 0.0}; + TrajectoryBounds bounds(3.0, 2.0, 1.0); + DoubleSTrajectory traj(state, bounds); + + auto phases = traj.phase_durations(); + REQUIRE(phases.count("total") == 1); + REQUIRE(phases.count("acceleration") == 1); + REQUIRE(phases.count("constant_velocity") == 1); + REQUIRE(phases.count("deceleration") == 1); + REQUIRE(phases["total"] >= 0.0); + REQUIRE(phases["acceleration"] >= 0.0); + REQUIRE(phases["constant_velocity"] >= 0.0); + REQUIRE(phases["deceleration"] >= 0.0); +} + +TEST_CASE("DoubleSTrajectory edge cases", "[double_s]") { + SECTION("Zero displacement") { + StateParams state{5.0, 5.0, 0.0, 0.0}; + TrajectoryBounds bounds(2.0, 1.0, 0.5); + DoubleSTrajectory traj(state, bounds); + + auto r = traj.evaluate(0.0); + REQUIRE_THAT(r.position, WithinAbs(5.0, kNumericalAtol)); + } + + SECTION("Small displacement") { + StateParams state{0.0, 0.001, 0.0, 0.0}; + TrajectoryBounds bounds(1.0, 1.0, 1.0); + DoubleSTrajectory traj(state, bounds); + + REQUIRE(traj.duration() >= 0.0); + } + + SECTION("Negative displacement") { + StateParams state{10.0, 0.0, 0.0, 0.0}; + TrajectoryBounds bounds(2.0, 1.0, 0.5); + DoubleSTrajectory traj(state, bounds); + + auto rT = traj.evaluate(traj.duration()); + REQUIRE_THAT(rT.position, WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Non-zero initial/final velocities") { + StateParams state{0.0, 10.0, 1.0, 2.0}; + TrajectoryBounds bounds(5.0, 3.0, 2.0); + DoubleSTrajectory traj(state, bounds); + + auto r0 = traj.evaluate(0.0); + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Large displacement") { + StateParams state{0.0, 1000.0, 0.0, 0.0}; + TrajectoryBounds bounds(10.0, 5.0, 2.0); + DoubleSTrajectory traj(state, bounds); + + REQUIRE(traj.duration() > 0.0); + auto rT = traj.evaluate(traj.duration()); + REQUIRE_THAT(rT.position, WithinAbs(1000.0, 0.1)); + } +} diff --git a/cpp/tests/test_parabolic_blend_trajectory.cpp b/cpp/tests/test_parabolic_blend_trajectory.cpp new file mode 100644 index 0000000..644d4d0 --- /dev/null +++ b/cpp/tests/test_parabolic_blend_trajectory.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::motion; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("ParabolicBlendTrajectory construction", "[parabolic_blend]") { + SECTION("Basic construction") { + std::vector q = {0, 1, 2}; + std::vector t = {0, 1, 2}; + std::vector dt = {0.1, 0.2, 0.1}; + ParabolicBlendTrajectory traj(q, t, dt); + + REQUIRE(traj.n_waypoints() == 3); + } + + SECTION("Single point") { + std::vector q = {1.0}; + std::vector t = {0.0}; + std::vector dt = {0.1}; + ParabolicBlendTrajectory traj(q, t, dt); + + REQUIRE(traj.n_waypoints() == 1); + } + + SECTION("Mismatched lengths") { + std::vector q = {0, 1, 2}; + std::vector t = {0, 1}; + std::vector dt = {0.1, 0.2, 0.1}; + REQUIRE_THROWS_AS(ParabolicBlendTrajectory(q, t, dt), + std::invalid_argument); + } +} + +TEST_CASE("ParabolicBlendTrajectory two-point", "[parabolic_blend]") { + std::vector q = {0, 1}; + std::vector t = {0, 1}; + std::vector dt = {0.2, 0.2}; + ParabolicBlendTrajectory traj(q, t, dt); + + REQUIRE(traj.duration() > 0.0); + + auto r = traj.evaluate(0.5); + REQUIRE(std::isfinite(r.position)); + REQUIRE(std::isfinite(r.velocity)); + REQUIRE(std::isfinite(r.acceleration)); +} + +TEST_CASE("ParabolicBlendTrajectory three-point", "[parabolic_blend]") { + std::vector q = {0, 1, 2}; + std::vector t = {0, 1, 2}; + std::vector dt = {0.1, 0.2, 0.1}; + ParabolicBlendTrajectory traj(q, t, dt); + + // Evaluate at several points + for (int i = 0; i <= 10; ++i) { + double eval_t = -0.05 + 2.05 * i / 10.0; + eval_t = std::clamp(eval_t, -0.05, 2.05); + auto r = traj.evaluate(eval_t); + REQUIRE(std::isfinite(r.position)); + } +} + +TEST_CASE("ParabolicBlendTrajectory position continuity", "[parabolic_blend]") { + std::vector q = {0, 2, 1, 4}; + std::vector t = {0, 1, 2, 3}; + std::vector dt = {0.2, 0.3, 0.3, 0.2}; + ParabolicBlendTrajectory traj(q, t, dt); + + // Check that position varies smoothly + double prev_pos = traj.evaluate(-0.1).position; + double max_jump = 0.0; + + for (int i = 1; i <= 100; ++i) { + double ti = -0.1 + 3.2 * i / 100.0; + auto r = traj.evaluate(ti); + double jump = std::abs(r.position - prev_pos); + max_jump = std::max(max_jump, jump); + prev_pos = r.position; + } + + // Should be relatively smooth (no huge jumps) + REQUIRE(max_jump < 1.0); +} + +TEST_CASE("ParabolicBlendTrajectory identical waypoints", "[parabolic_blend]") { + std::vector q = {1, 1, 1}; + std::vector t = {0, 1, 2}; + std::vector dt = {0.1, 0.2, 0.1}; + ParabolicBlendTrajectory traj(q, t, dt); + + auto r = traj.evaluate(1.0); + REQUIRE(std::isfinite(r.position)); + REQUIRE(std::isfinite(r.velocity)); +} + +TEST_CASE("ParabolicBlendTrajectory negative positions", "[parabolic_blend]") { + std::vector q = {-2, -1, -3}; + std::vector t = {0, 1, 2}; + std::vector dt = {0.1, 0.2, 0.1}; + ParabolicBlendTrajectory traj(q, t, dt); + + auto r = traj.evaluate(0.5); + REQUIRE(std::isfinite(r.position)); +} + +TEST_CASE("ParabolicBlendTrajectory single waypoint", "[parabolic_blend]") { + std::vector q = {5.0}; + std::vector t = {0.0}; + std::vector dt = {0.2}; + ParabolicBlendTrajectory traj(q, t, dt); + + auto r = traj.evaluate(0.0); + REQUIRE(std::isfinite(r.position)); + REQUIRE_THAT(r.velocity, WithinAbs(0.0, kNumericalAtol)); +} diff --git a/cpp/tests/test_paths.cpp b/cpp/tests/test_paths.cpp new file mode 100644 index 0000000..49f1cf0 --- /dev/null +++ b/cpp/tests/test_paths.cpp @@ -0,0 +1,292 @@ +#include +#include + +#include +#include +#include +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::path; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +// ===== LinearPath ===== + +TEST_CASE("LinearPath construction", "[linear_path]") { + Eigen::Vector3d pi(0, 0, 0); + Eigen::Vector3d pf(1, 0, 0); + LinearPath path(pi, pf); + + REQUIRE_THAT(path.length(), WithinAbs(1.0, kRegularAtol)); +} + +TEST_CASE("LinearPath position", "[linear_path]") { + Eigen::Vector3d pi(0, 0, 0); + Eigen::Vector3d pf(2, 0, 0); + LinearPath path(pi, pf); + + auto p0 = path.position(0.0); + auto pm = path.position(1.0); + auto p1 = path.position(2.0); + + REQUIRE_THAT(p0(0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(pm(0), WithinAbs(1.0, kRegularAtol)); + REQUIRE_THAT(p1(0), WithinAbs(2.0, kRegularAtol)); +} + +TEST_CASE("LinearPath velocity and acceleration", "[linear_path]") { + Eigen::Vector3d pi(0, 0, 0); + Eigen::Vector3d pf(1, 1, 1); + LinearPath path(pi, pf); + + auto v = path.velocity(0.5); + auto a = path.acceleration(0.5); + + double expected_v = 1.0 / std::sqrt(3.0); + REQUIRE_THAT(v(0), WithinAbs(expected_v, kNumericalAtol)); + REQUIRE_THAT(a.norm(), WithinAbs(0.0, kRegularAtol)); +} + +TEST_CASE("LinearPath vector evaluation", "[linear_path]") { + Eigen::Vector3d pi(0, 0, 0); + Eigen::Vector3d pf(3, 0, 0); + LinearPath path(pi, pf); + + Eigen::VectorXd s(3); + s << 0, 1.5, 3; + auto pts = path.position(s); + + REQUIRE(pts.rows() == 3); + REQUIRE(pts.cols() == 3); + REQUIRE_THAT(pts(1, 0), WithinAbs(1.5, kRegularAtol)); +} + +TEST_CASE("LinearPath zero length", "[linear_path]") { + Eigen::Vector3d p(1, 2, 3); + LinearPath path(p, p); + + REQUIRE_THAT(path.length(), WithinAbs(0.0, kRegularAtol)); + auto pos = path.position(0.0); + REQUIRE_THAT(pos(0), WithinAbs(1.0, kRegularAtol)); +} + +// ===== CircularPath ===== + +TEST_CASE("CircularPath XY plane", "[circular_path]") { + Eigen::Vector3d axis(0, 0, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d circle_point(1, 0, 0); + CircularPath path(axis, axis_point, circle_point); + + REQUIRE_THAT(path.radius(), WithinAbs(1.0, kRegularAtol)); + + SECTION("Start point") { + auto p = path.position(0.0); + REQUIRE_THAT(p(0), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(p(1), WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Quarter circle") { + auto p = path.position(M_PI / 2.0); + REQUIRE_THAT(p(0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(p(1), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Half circle") { + auto p = path.position(M_PI); + REQUIRE_THAT(p(0), WithinAbs(-1.0, kNumericalAtol)); + REQUIRE_THAT(p(1), WithinAbs(0.0, kNumericalAtol)); + } +} + +TEST_CASE("CircularPath velocity", "[circular_path]") { + Eigen::Vector3d axis(0, 0, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d circle_point(1, 0, 0); + CircularPath path(axis, axis_point, circle_point); + + auto v = path.velocity(0.0); + // At s=0, velocity should be in +y direction + REQUIRE_THAT(v(0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(v(1), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(v.norm(), WithinAbs(1.0, kNumericalAtol)); +} + +TEST_CASE("CircularPath acceleration", "[circular_path]") { + Eigen::Vector3d axis(0, 0, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d circle_point(2, 0, 0); + CircularPath path(axis, axis_point, circle_point); + + auto a = path.acceleration(0.0); + // Centripetal: points toward center = -x direction, magnitude = 1/R + REQUIRE_THAT(a(0), WithinAbs(-0.5, kNumericalAtol)); // -1/R = -1/2 + REQUIRE_THAT(a(1), WithinAbs(0.0, kNumericalAtol)); +} + +TEST_CASE("CircularPath point on axis", "[circular_path]") { + Eigen::Vector3d axis(0, 0, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d on_axis(0, 0, 5); + + REQUIRE_THROWS_AS(CircularPath(axis, axis_point, on_axis), std::invalid_argument); +} + +TEST_CASE("CircularPath vector evaluation", "[circular_path]") { + Eigen::Vector3d axis(0, 0, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d circle_point(1, 0, 0); + CircularPath path(axis, axis_point, circle_point); + + Eigen::VectorXd s(4); + s << 0, M_PI / 2, M_PI, 3 * M_PI / 2; + auto pts = path.position(s); + + REQUIRE(pts.rows() == 4); + REQUIRE(pts.cols() == 3); +} + +TEST_CASE("CircularPath 3D", "[circular_path]") { + Eigen::Vector3d axis(1, 1, 1); + Eigen::Vector3d axis_point(0, 0, 0); + Eigen::Vector3d circle_point(1, 0, 0); + CircularPath path(axis, axis_point, circle_point); + + REQUIRE(path.radius() > 0.0); + auto p = path.position(0.5); + REQUIRE(p.allFinite()); +} + +// ===== Frenet Frame ===== + +TEST_CASE("Frenet frame linear path", "[frenet_frame]") { + auto curve = [](double s) -> std::tuple { + return {Eigen::Vector3d(s, 0, 0), Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(0, 0, 0)}; + }; + + Eigen::VectorXd s(3); + s << 0, 1, 2; + auto frames = compute_frenet_frames(curve, s); + + REQUIRE(frames.size() == 3); + REQUIRE_THAT(frames[0].curvature, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(frames[0].tangent(0), WithinAbs(1.0, kNumericalAtol)); +} + +TEST_CASE("Frenet frame circular path", "[frenet_frame]") { + auto curve = [](double s) -> std::tuple { + return {Eigen::Vector3d(std::cos(s), std::sin(s), 0), + Eigen::Vector3d(-std::sin(s), std::cos(s), 0), + Eigen::Vector3d(-std::cos(s), -std::sin(s), 0)}; + }; + + Eigen::VectorXd s(1); + s << 0; + auto frames = compute_frenet_frames(curve, s); + + REQUIRE_THAT(frames[0].curvature, WithinAbs(1.0, kNumericalAtol)); +} + +// ===== Frenet Helper Functions ===== + +TEST_CASE("circular_trajectory_with_derivatives", "[frenet_helpers]") { + SECTION("At u=0") { + auto [p, dp, d2p] = circular_trajectory_with_derivatives(0.0, 2.0); + + REQUIRE_THAT(p(0), WithinAbs(2.0, kRegularAtol)); + REQUIRE_THAT(p(1), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p(2), WithinAbs(0.0, kRegularAtol)); + + REQUIRE_THAT(dp(0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(dp(1), WithinAbs(2.0, kRegularAtol)); + REQUIRE_THAT(dp(2), WithinAbs(0.0, kRegularAtol)); + + REQUIRE_THAT(d2p(0), WithinAbs(-2.0, kRegularAtol)); + REQUIRE_THAT(d2p(1), WithinAbs(0.0, kRegularAtol)); + } + + SECTION("At u=PI/2") { + auto [p, dp, d2p] = circular_trajectory_with_derivatives(M_PI / 2.0, 1.0); + + REQUIRE_THAT(p(0), WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(p(1), WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("helicoidal_trajectory_with_derivatives", "[frenet_helpers]") { + SECTION("At u=0") { + auto [p, dp, d2p] = helicoidal_trajectory_with_derivatives(0.0, 2.0, 0.5); + + REQUIRE_THAT(p(0), WithinAbs(2.0, kRegularAtol)); + REQUIRE_THAT(p(1), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(p(2), WithinAbs(0.0, kRegularAtol)); + + REQUIRE_THAT(dp(2), WithinAbs(0.5, kRegularAtol)); // z-velocity = d + } + + SECTION("Z component grows linearly") { + double u = 3.0; + double d = 0.5; + auto [p, dp, d2p] = helicoidal_trajectory_with_derivatives(u, 2.0, d); + + REQUIRE_THAT(p(2), WithinAbs(d * u, kRegularAtol)); + REQUIRE_THAT(d2p(2), WithinAbs(0.0, kRegularAtol)); // No z-acceleration + } +} + +TEST_CASE("Frenet frames with helicoidal helper", "[frenet_helpers]") { + Eigen::VectorXd s(5); + for (int i = 0; i < 5; ++i) s[i] = M_PI * i / 4.0; + + auto curve = [](double u) { + return helicoidal_trajectory_with_derivatives(u, 2.0, 0.5); + }; + + auto frames = compute_frenet_frames(curve, s); + REQUIRE(frames.size() == 5); + + for (const auto& f : frames) { + REQUIRE(f.tangent.allFinite()); + REQUIRE(f.curvature >= 0.0); + } +} + +// ===== Linear Trajectory ===== + +TEST_CASE("Linear trajectory 1D", "[linear_traj]") { + Eigen::VectorXd p0(1); + p0 << 0; + Eigen::VectorXd p1(1); + p1 << 10; + + auto result = linear_traj(p0, p1, 0.0, 5.0, 6); + + REQUIRE(result.positions.rows() == 6); + REQUIRE_THAT(result.positions(0, 0), WithinAbs(0.0, kRegularAtol)); + REQUIRE_THAT(result.positions(5, 0), WithinAbs(10.0, kRegularAtol)); + REQUIRE_THAT(result.velocities(0, 0), WithinAbs(2.0, kRegularAtol)); +} + +TEST_CASE("Linear trajectory 3D", "[linear_traj]") { + Eigen::VectorXd p0(3); + p0 << 0, 0, 0; + Eigen::VectorXd p1(3); + p1 << 1, 2, 3; + + auto result = linear_traj(p0, p1, 0.0, 1.0, 11); + + REQUIRE(result.positions.rows() == 11); + REQUIRE(result.positions.cols() == 3); + + // Midpoint + REQUIRE_THAT(result.positions(5, 0), WithinAbs(0.5, kRegularAtol)); + REQUIRE_THAT(result.positions(5, 1), WithinAbs(1.0, kRegularAtol)); + REQUIRE_THAT(result.positions(5, 2), WithinAbs(1.5, kRegularAtol)); + + // Zero acceleration + REQUIRE_THAT(result.accelerations.norm(), WithinAbs(0.0, kRegularAtol)); +} diff --git a/cpp/tests/test_polynomial_trajectory.cpp b/cpp/tests/test_polynomial_trajectory.cpp new file mode 100644 index 0000000..e978dd5 --- /dev/null +++ b/cpp/tests/test_polynomial_trajectory.cpp @@ -0,0 +1,223 @@ +#include +#include + +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::motion; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +// --- Order 3 --- + +TEST_CASE("Order 3 basic trajectory", "[polynomial]") { + BoundaryCondition start{0.0, 0.0}; + BoundaryCondition end{10.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + PolynomialTrajectory traj(start, end, interval, 3); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(2.0); + + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(10.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(0.0, kNumericalAtol)); +} + +TEST_CASE("Order 3 nonzero velocities", "[polynomial]") { + BoundaryCondition start{2.0, 1.0}; + BoundaryCondition end{8.0, 3.0}; + TimeInterval interval{0.0, 3.0}; + PolynomialTrajectory traj(start, end, interval, 3); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(3.0); + + REQUIRE_THAT(r0.position, WithinAbs(2.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(8.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(3.0, kNumericalAtol)); +} + +TEST_CASE("Order 3 negative displacement", "[polynomial]") { + BoundaryCondition start{10.0, 0.0}; + BoundaryCondition end{3.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + PolynomialTrajectory traj(start, end, interval, 3); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(2.0); + + REQUIRE_THAT(r0.position, WithinAbs(10.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(3.0, kNumericalAtol)); +} + +TEST_CASE("Order 3 jerk constant", "[polynomial]") { + BoundaryCondition start{0.0, 0.0}; + BoundaryCondition end{10.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + PolynomialTrajectory traj(start, end, interval, 3); + + // Jerk should be constant for cubic + double j0 = traj.evaluate(0.0).jerk; + double j1 = traj.evaluate(0.5).jerk; + double j2 = traj.evaluate(1.5).jerk; + + REQUIRE_THAT(j0, WithinAbs(j1, kNumericalAtol)); + REQUIRE_THAT(j1, WithinAbs(j2, kNumericalAtol)); +} + +// --- Order 5 --- + +TEST_CASE("Order 5 basic trajectory", "[polynomial]") { + BoundaryCondition start{0.0, 0.0, 0.0}; + BoundaryCondition end{8.0, 0.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + PolynomialTrajectory traj(start, end, interval, 5); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(2.0); + + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.acceleration, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(8.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r1.acceleration, WithinAbs(0.0, kNumericalAtol)); +} + +TEST_CASE("Order 5 nonzero accelerations", "[polynomial]") { + BoundaryCondition start{1.0, 2.0, 0.5}; + BoundaryCondition end{9.0, 1.0, -0.3}; + TimeInterval interval{0.0, 4.0}; + PolynomialTrajectory traj(start, end, interval, 5); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(4.0); + + REQUIRE_THAT(r0.position, WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(2.0, kNumericalAtol)); + REQUIRE_THAT(r0.acceleration, WithinAbs(0.5, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(9.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(r1.acceleration, WithinAbs(-0.3, kNumericalAtol)); +} + +// --- Order 7 --- + +TEST_CASE("Order 7 basic trajectory", "[polynomial]") { + BoundaryCondition start{0.0, 0.0, 0.0, 0.0}; + BoundaryCondition end{10.0, 0.0, 0.0, 0.0}; + TimeInterval interval{0.0, 3.0}; + PolynomialTrajectory traj(start, end, interval, 7); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(3.0); + + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.acceleration, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r0.jerk, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(10.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(0.0, kNumericalAtol)); +} + +TEST_CASE("Order 7 nonzero jerks", "[polynomial]") { + BoundaryCondition start{1.0, 1.0, 0.0, 0.5}; + BoundaryCondition end{11.0, 2.0, -1.0, -0.3}; + TimeInterval interval{0.0, 4.0}; + PolynomialTrajectory traj(start, end, interval, 7); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(4.0); + + REQUIRE_THAT(r0.position, WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(r0.velocity, WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(r0.jerk, WithinAbs(0.5, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(11.0, kNumericalAtol)); + REQUIRE_THAT(r1.velocity, WithinAbs(2.0, kNumericalAtol)); +} + +// --- Heuristic velocities --- + +TEST_CASE("Heuristic velocities", "[polynomial]") { + SECTION("Basic") { + std::vector pts = {0, 5, 10}; + std::vector times = {0, 2, 4}; + auto v = PolynomialTrajectory::heuristic_velocities(pts, times); + REQUIRE(v.size() == 3); + REQUIRE_THAT(v[0], WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(v[2], WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Minimum points") { + std::vector pts = {0}; + std::vector times = {0}; + REQUIRE_THROWS_AS(PolynomialTrajectory::heuristic_velocities(pts, times), + std::invalid_argument); + } + + SECTION("Direction change") { + std::vector pts = {0, 5, 3, 8}; + std::vector times = {0, 1, 2, 3}; + auto v = PolynomialTrajectory::heuristic_velocities(pts, times); + // At direction change (5โ†’3), velocity should be 0 + REQUIRE_THAT(v[2], WithinAbs(0.0, kNumericalAtol)); + } +} + +// --- Multipoint --- + +TEST_CASE("Multipoint trajectory", "[polynomial]") { + std::vector pts = {0, 5, 10}; + std::vector times = {0, 2, 4}; + + SECTION("Order 3") { + auto segments = PolynomialTrajectory::multipoint_trajectory(pts, times, 3); + REQUIRE(segments.size() == 2); + + auto r = PolynomialTrajectory::evaluate_multipoint(segments, 1.0); + REQUIRE(std::isfinite(r.position)); + } + + SECTION("Order 5") { + auto segments = PolynomialTrajectory::multipoint_trajectory(pts, times, 5); + auto r = PolynomialTrajectory::evaluate_multipoint(segments, 1.0); + REQUIRE(std::isfinite(r.position)); + } + + SECTION("Boundary evaluation") { + auto segments = PolynomialTrajectory::multipoint_trajectory(pts, times, 3); + auto r0 = PolynomialTrajectory::evaluate_multipoint(segments, 0.0); + auto r1 = PolynomialTrajectory::evaluate_multipoint(segments, 4.0); + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(10.0, kNumericalAtol)); + } +} + +// --- Edge cases --- + +TEST_CASE("Polynomial edge cases", "[polynomial]") { + SECTION("Invalid order") { + BoundaryCondition start{0.0, 0.0}; + BoundaryCondition end{10.0, 0.0}; + TimeInterval interval{0.0, 2.0}; + REQUIRE_THROWS_AS(PolynomialTrajectory(start, end, interval, 4), + std::invalid_argument); + } + + SECTION("Zero displacement") { + BoundaryCondition start{5.0, 2.0}; + BoundaryCondition end{5.0, -2.0}; + TimeInterval interval{0.0, 1.0}; + PolynomialTrajectory traj(start, end, interval, 3); + + auto r0 = traj.evaluate(0.0); + auto r1 = traj.evaluate(1.0); + REQUIRE_THAT(r0.position, WithinAbs(5.0, kNumericalAtol)); + REQUIRE_THAT(r1.position, WithinAbs(5.0, kNumericalAtol)); + } +} diff --git a/cpp/tests/test_quaternion.cpp b/cpp/tests/test_quaternion.cpp new file mode 100644 index 0000000..e8d0864 --- /dev/null +++ b/cpp/tests/test_quaternion.cpp @@ -0,0 +1,161 @@ +#include +#include + +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::quat; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("Quaternion construction", "[quaternion]") { + SECTION("Default is identity") { + Quaternion q; + REQUIRE_THAT(q.w(), WithinAbs(1.0, kRegularAtol)); + REQUIRE_THAT(q.x(), WithinAbs(0.0, kRegularAtol)); + } + + SECTION("Identity factory") { + auto q = Quaternion::identity(); + REQUIRE_THAT(q.norm(), WithinAbs(1.0, kRegularAtol)); + } + + SECTION("From angle-axis") { + auto q = Quaternion::from_angle_axis(M_PI / 2, Eigen::Vector3d::UnitZ()); + REQUIRE_THAT(q.norm(), WithinAbs(1.0, kRegularAtol)); + REQUIRE_THAT(q.w(), WithinAbs(std::cos(M_PI / 4), kNumericalAtol)); + } + + SECTION("From euler angles") { + auto q = Quaternion::from_euler_angles(0.1, 0.2, 0.3); + REQUIRE_THAT(q.norm(), WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("Quaternion arithmetic", "[quaternion]") { + auto q1 = Quaternion::from_angle_axis(M_PI / 4, Eigen::Vector3d::UnitZ()); + auto q2 = Quaternion::from_angle_axis(M_PI / 4, Eigen::Vector3d::UnitX()); + + SECTION("Multiplication") { + auto q3 = q1 * q2; + REQUIRE_THAT(q3.norm(), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Conjugate") { + auto qc = q1.conjugate(); + REQUIRE_THAT(qc.w(), WithinAbs(q1.w(), kRegularAtol)); + REQUIRE_THAT(qc.x(), WithinAbs(-q1.x(), kRegularAtol)); + } + + SECTION("Inverse") { + auto qi = q1.inverse(); + auto prod = q1 * qi; + REQUIRE_THAT(prod.w(), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(prod.x(), WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Dot product") { + double d = q1.dot_product(q1); + REQUIRE_THAT(d, WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("Quaternion exp/log", "[quaternion]") { + SECTION("Exp of zero") { + Quaternion zero(0, 0, 0, 0); + auto result = Quaternion::exp(zero); + REQUIRE_THAT(result.w(), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Log of identity") { + auto result = Quaternion::log(Quaternion::identity()); + REQUIRE_THAT(result.w(), WithinAbs(0.0, kNumericalAtol)); + REQUIRE(result.vec().norm() < kNumericalAtol); + } + + SECTION("Exp(log(q)) = q") { + auto q = Quaternion::from_angle_axis(1.0, Eigen::Vector3d::UnitZ()); + auto result = Quaternion::exp(Quaternion::log(q)); + REQUIRE_THAT(result.w(), WithinAbs(q.w(), kNumericalAtol)); + REQUIRE_THAT(result.x(), WithinAbs(q.x(), kNumericalAtol)); + } + + SECTION("Power") { + auto q = Quaternion::from_angle_axis(M_PI / 2, Eigen::Vector3d::UnitZ()); + auto q_half = Quaternion::power(q, 0.5); + REQUIRE_THAT(q_half.norm(), WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("Quaternion SLERP", "[quaternion]") { + auto q0 = Quaternion::identity(); + auto q1 = Quaternion::from_angle_axis(M_PI / 2, Eigen::Vector3d::UnitZ()); + + SECTION("Endpoints") { + auto r0 = Quaternion::slerp(q0, q1, 0.0); + auto r1 = Quaternion::slerp(q0, q1, 1.0); + REQUIRE_THAT(std::abs(r0.dot_product(q0)), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(std::abs(r1.dot_product(q1)), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Unit norm throughout") { + for (int i = 0; i <= 10; ++i) { + double t = i / 10.0; + auto r = Quaternion::slerp(q0, q1, t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } + } + + SECTION("Double-cover handling") { + auto q_neg = -q1; + auto r = Quaternion::slerp(q0, q_neg, 0.5); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("Quaternion SQUAD", "[quaternion]") { + auto p = Quaternion::identity(); + auto q = Quaternion::from_angle_axis(M_PI / 2, Eigen::Vector3d::UnitZ()); + auto a = Quaternion::slerp(p, q, 0.25); + auto b = Quaternion::slerp(p, q, 0.75); + + SECTION("Endpoints") { + auto r0 = Quaternion::squad(p, a, b, q, 0.0); + auto r1 = Quaternion::squad(p, a, b, q, 1.0); + REQUIRE_THAT(std::abs(r0.dot_product(p)), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(std::abs(r1.dot_product(q)), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Unit norm") { + for (int i = 0; i <= 10; ++i) { + auto r = Quaternion::squad(p, a, b, q, i / 10.0); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } + } +} + +TEST_CASE("Quaternion intermediate", "[quaternion]") { + auto q0 = Quaternion::identity(); + auto q1 = Quaternion::from_euler_angles(0.1, 0.2, 0.3); + auto q2 = Quaternion::from_euler_angles(0.2, 0.4, 0.6); + + auto s = Quaternion::compute_intermediate_quaternion(q0, q1, q2); + REQUIRE_THAT(s.norm(), WithinAbs(1.0, kNumericalAtol)); +} + +TEST_CASE("Quaternion conversions", "[quaternion]") { + auto q = Quaternion::from_angle_axis(M_PI / 3, Eigen::Vector3d(1, 1, 0).normalized()); + + SECTION("Rotation matrix") { + auto R = q.to_rotation_matrix(); + REQUIRE_THAT(R.determinant(), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Axis-angle roundtrip") { + auto [axis, angle] = q.to_axis_angle(); + auto q2 = Quaternion::from_angle_axis(angle, axis); + REQUIRE_THAT(std::abs(q.dot_product(q2)), WithinAbs(1.0, kNumericalAtol)); + } +} diff --git a/cpp/tests/test_quaternion_spline.cpp b/cpp/tests/test_quaternion_spline.cpp new file mode 100644 index 0000000..6e57ee4 --- /dev/null +++ b/cpp/tests/test_quaternion_spline.cpp @@ -0,0 +1,334 @@ +#include +#include + +#include +#include +#include +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::quat; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +// Helper: create test quaternion sequence +static std::vector make_test_quats(int n = 5) { + std::vector quats; + for (int i = 0; i < n; ++i) { + double a = 0.1 * (i + 1); + quats.push_back(Quaternion::from_euler_angles(a, a * 2, a * 3)); + } + return quats; +} + +static std::vector make_test_times(int n = 5) { + std::vector times(n); + for (int i = 0; i < n; ++i) times[i] = static_cast(i); + return times; +} + +// ===== QuaternionSpline ===== + +TEST_CASE("QuaternionSpline SLERP", "[quat_spline]") { + auto times = make_test_times(); + auto quats = make_test_quats(); + QuaternionSpline spline(times, quats, QuaternionSpline::Method::Slerp); + + SECTION("Endpoints") { + auto r0 = spline.evaluate(0.0); + REQUIRE_THAT(std::abs(r0.dot_product(quats[0])), WithinAbs(1.0, kNumericalAtol)); + + auto rn = spline.evaluate(4.0); + REQUIRE_THAT(std::abs(rn.dot_product(quats[4])), WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Unit norm throughout") { + for (int i = 0; i <= 20; ++i) { + double t = 4.0 * i / 20.0; + auto r = spline.evaluate(t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } + } + + SECTION("Velocity finite") { + auto v = spline.evaluate_velocity(2.0); + REQUIRE(v.allFinite()); + } + + SECTION("Acceleration finite") { + auto a = spline.evaluate_acceleration(2.0); + REQUIRE(a.allFinite()); + } +} + +TEST_CASE("QuaternionSpline SQUAD", "[quat_spline]") { + auto times = make_test_times(); + auto quats = make_test_quats(); + QuaternionSpline spline(times, quats, QuaternionSpline::Method::Squad); + + for (int i = 0; i <= 20; ++i) { + double t = 4.0 * i / 20.0; + auto r = spline.evaluate(t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } +} + +TEST_CASE("QuaternionSpline Auto", "[quat_spline]") { + auto times = make_test_times(); + auto quats = make_test_quats(); + QuaternionSpline spline(times, quats, QuaternionSpline::Method::Auto); + + auto r = spline.evaluate(2.0); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); +} + +TEST_CASE("QuaternionSpline validation", "[quat_spline]") { + SECTION("Mismatched sizes") { + std::vector t = {0, 1}; + std::vector q = {Quaternion::identity()}; + REQUIRE_THROWS_AS(QuaternionSpline(t, q), std::invalid_argument); + } + + SECTION("Too few points") { + std::vector t = {0}; + std::vector q = {Quaternion::identity()}; + REQUIRE_THROWS_AS(QuaternionSpline(t, q), std::invalid_argument); + } +} + +// ===== SquadC2 ===== + +TEST_CASE("SquadC2 construction", "[squad_c2]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + SquadC2 spline(times, quats); + + SECTION("Unit norm throughout") { + for (int i = 0; i <= 20; ++i) { + double t = 5.0 * i / 20.0; + auto r = spline.evaluate(t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); + } + } +} + +TEST_CASE("SquadC2 endpoints", "[squad_c2]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + SquadC2 spline(times, quats); + + auto r0 = spline.evaluate(0.0); + auto rn = spline.evaluate(5.0); + + REQUIRE_THAT(std::abs(r0.dot_product(quats[0])), WithinAbs(1.0, kNumericalAtol)); + REQUIRE_THAT(std::abs(rn.dot_product(quats[5])), WithinAbs(1.0, kNumericalAtol)); +} + +TEST_CASE("SquadC2 validation", "[squad_c2]") { + SECTION("Non-monotonic times") { + std::vector t = {0, 1, 0.5, 3}; + auto quats = make_test_quats(4); + REQUIRE_THROWS_AS(SquadC2(t, quats), std::invalid_argument); + } +} + +TEST_CASE("SquadC2 zero-clamped boundaries", "[squad_c2]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + SquadC2 spline(times, quats); + + // Angular velocity should be near-zero at boundaries + auto v_start = spline.evaluate_velocity(0.0); + auto v_end = spline.evaluate_velocity(5.0); + + REQUIRE(v_start.norm() < 0.1); + REQUIRE(v_end.norm() < 0.1); +} + +TEST_CASE("SquadC2 velocity and acceleration", "[squad_c2]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + SquadC2 spline(times, quats); + + auto v = spline.evaluate_velocity(2.5); + auto a = spline.evaluate_acceleration(2.5); + + REQUIRE(v.allFinite()); + REQUIRE(a.allFinite()); +} + +// ===== LogQuaternionInterpolation ===== + +TEST_CASE("LogQuaternionInterpolation construction", "[log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + LogQuaternionInterpolation lqi(times, quats, 3); + + SECTION("Unit norm throughout") { + for (int i = 0; i <= 20; ++i) { + double t = 5.0 * i / 20.0; + auto r = lqi.evaluate(t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, 1e-4)); + } + } +} + +TEST_CASE("LogQuaternionInterpolation endpoints", "[log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + LogQuaternionInterpolation lqi(times, quats, 3); + + auto r0 = lqi.evaluate(0.0); + auto rn = lqi.evaluate(5.0); + + REQUIRE_THAT(std::abs(r0.dot_product(quats[0])), WithinAbs(1.0, 1e-4)); + REQUIRE_THAT(std::abs(rn.dot_product(quats[5])), WithinAbs(1.0, 1e-4)); +} + +TEST_CASE("LogQuaternionInterpolation velocity", "[log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + LogQuaternionInterpolation lqi(times, quats, 3); + + auto v = lqi.evaluate_velocity(2.5); + auto a = lqi.evaluate_acceleration(2.5); + + REQUIRE(v.allFinite()); + REQUIRE(a.allFinite()); +} + +TEST_CASE("LogQuaternionInterpolation validation", "[log_quat]") { + SECTION("Too few points") { + std::vector t = {0}; + std::vector q = {Quaternion::identity()}; + REQUIRE_THROWS_AS(LogQuaternionInterpolation(t, q), std::invalid_argument); + } + + SECTION("Invalid degree") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + REQUIRE_THROWS_AS(LogQuaternionInterpolation(times, quats, 2), std::invalid_argument); + } + + SECTION("Non-monotonic times") { + std::vector t = {0, 1, 0.5, 3}; + auto quats = make_test_quats(4); + REQUIRE_THROWS_AS(LogQuaternionInterpolation(t, quats), std::invalid_argument); + } +} + +// ===== ModifiedLogQuaternionInterpolation ===== + +TEST_CASE("ModifiedLogQuaternionInterpolation construction", "[mod_log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + ModifiedLogQuaternionInterpolation mlqi(times, quats, 3); + + SECTION("Unit norm throughout") { + for (int i = 0; i <= 20; ++i) { + double t = 5.0 * i / 20.0; + auto r = mlqi.evaluate(t); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, 1e-4)); + } + } +} + +TEST_CASE("ModifiedLogQuaternionInterpolation endpoints", "[mod_log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + ModifiedLogQuaternionInterpolation mlqi(times, quats, 3); + + auto r0 = mlqi.evaluate(0.0); + auto rn = mlqi.evaluate(5.0); + + REQUIRE_THAT(std::abs(r0.dot_product(quats[0])), WithinAbs(1.0, 1e-4)); + REQUIRE_THAT(std::abs(rn.dot_product(quats[5])), WithinAbs(1.0, 1e-4)); +} + +TEST_CASE("ModifiedLogQuaternionInterpolation velocity returns 4D", "[mod_log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + ModifiedLogQuaternionInterpolation mlqi(times, quats, 3); + + auto v = mlqi.evaluate_velocity(2.5); + auto a = mlqi.evaluate_acceleration(2.5); + + REQUIRE(v.size() == 4); + REQUIRE(a.size() == 4); + REQUIRE(v.allFinite()); + REQUIRE(a.allFinite()); +} + +TEST_CASE("ModifiedLogQuaternionInterpolation without normalization", "[mod_log_quat]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + ModifiedLogQuaternionInterpolation mlqi(times, quats, 3, false); + + REQUIRE_FALSE(mlqi.normalize_axis()); + + for (int i = 0; i <= 10; ++i) { + double t = 5.0 * i / 10.0; + auto r = mlqi.evaluate(t); + REQUIRE(r.norm() > 0.0); + REQUIRE(std::isfinite(r.norm())); + } +} + +TEST_CASE("ModifiedLogQuaternionInterpolation validation", "[mod_log_quat]") { + SECTION("Mismatched sizes") { + std::vector t = {0, 1}; + std::vector q = {Quaternion::identity()}; + REQUIRE_THROWS_AS(ModifiedLogQuaternionInterpolation(t, q), std::invalid_argument); + } + + SECTION("Too few points") { + std::vector t = {0}; + std::vector q = {Quaternion::identity()}; + REQUIRE_THROWS_AS(ModifiedLogQuaternionInterpolation(t, q), std::invalid_argument); + } + + SECTION("Invalid degree") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + REQUIRE_THROWS_AS(ModifiedLogQuaternionInterpolation(times, quats, 2), + std::invalid_argument); + REQUIRE_THROWS_AS(ModifiedLogQuaternionInterpolation(times, quats, 6), + std::invalid_argument); + } + + SECTION("Non-monotonic times") { + std::vector t = {0, 1, 0.5, 3}; + auto quats = make_test_quats(4); + REQUIRE_THROWS_AS(ModifiedLogQuaternionInterpolation(t, quats), std::invalid_argument); + } + + SECTION("Wrong velocity size") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + Eigen::VectorXd bad_vel(3); + bad_vel << 0, 0, 0; + REQUIRE_THROWS_AS( + ModifiedLogQuaternionInterpolation(times, quats, 3, true, bad_vel), + std::invalid_argument); + } +} + +// ===== SquadC2 Config ===== + +TEST_CASE("SquadC2 config constructor", "[squad_c2]") { + auto times = make_test_times(6); + auto quats = make_test_quats(6); + + SquadC2Config config; + config.time_points = times; + config.quaternions = quats; + config.validate_continuity = true; + + SquadC2 spline(config); + + REQUIRE(spline.validate_continuity()); + auto r = spline.evaluate(2.5); + REQUIRE_THAT(r.norm(), WithinAbs(1.0, kNumericalAtol)); +} diff --git a/cpp/tests/test_smoothing_search.cpp b/cpp/tests/test_smoothing_search.cpp new file mode 100644 index 0000000..4503265 --- /dev/null +++ b/cpp/tests/test_smoothing_search.cpp @@ -0,0 +1,129 @@ +#include +#include + +#include + +#include "test_data.hpp" + +#include +#include + +using namespace interpolatecpp::spline; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +namespace { +// Generate noisy sine data +void make_noisy_sine(std::vector& t, std::vector& q, int n, double noise) { + t.resize(static_cast(n)); + q.resize(static_cast(n)); + // Simple deterministic "noise" using a fixed pattern + for (int i = 0; i < n; ++i) { + t[static_cast(i)] = 2.0 * M_PI * i / (n - 1); + double pseudo_noise = noise * std::sin(17.0 * i + 0.3) * std::cos(31.0 * i + 0.7); + q[static_cast(i)] = std::sin(t[static_cast(i)]) + pseudo_noise; + } +} +} // namespace + +TEST_CASE("Smoothing spline with tolerance - basic search", "[smoothing_search]") { + std::vector t, q; + make_noisy_sine(t, q, 30, 0.1); + + SplineConfig config; + config.max_iterations = 30; + + SECTION("Finds spline within tolerance") { + double tolerance = 0.2; + auto result = smoothing_spline_with_tolerance(t, q, tolerance, config); + + REQUIRE(result.max_error <= tolerance + kNumericalAtol); + REQUIRE(result.mu > 0.0); + REQUIRE(result.mu <= 1.0); + REQUIRE(result.iterations > 0); + REQUIRE(result.iterations <= config.max_iterations); + } + + SECTION("Tight tolerance gives mu close to 1") { + double tolerance = 1e-10; + auto result = smoothing_spline_with_tolerance(t, q, tolerance, config); + + REQUIRE(result.mu > 0.9); + } + + SECTION("Loose tolerance gives lower mu") { + double tolerance = 1.0; + auto result = smoothing_spline_with_tolerance(t, q, tolerance, config); + + REQUIRE(result.mu < 0.5); + } +} + +TEST_CASE("Smoothing spline with tolerance - convergence", "[smoothing_search]") { + std::vector t, q; + make_noisy_sine(t, q, 20, 0.15); + + SECTION("Converges within max iterations") { + SplineConfig config; + config.max_iterations = 50; + double tolerance = 0.1; + + auto result = smoothing_spline_with_tolerance(t, q, tolerance, config); + REQUIRE(result.iterations <= 50); + } + + SECTION("Max iterations reached returns best solution") { + SplineConfig config; + config.max_iterations = 3; // Very few iterations + double tolerance = 0.05; + + auto result = smoothing_spline_with_tolerance(t, q, tolerance, config); + // Should return something valid + REQUIRE(result.mu > 0.0); + REQUIRE(std::isfinite(result.max_error)); + } +} + +TEST_CASE("Smoothing spline with tolerance - with weights", "[smoothing_search]") { + std::vector t = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0}; + std::vector q = {0.0, 1.1, 0.9, 0.1, -0.9, -1.0}; + + SplineConfig config; + config.max_iterations = 30; + config.weights = (Eigen::VectorXd(6) << 10.0, 1.0, 1.0, 1.0, 1.0, 10.0).finished(); + + auto result = smoothing_spline_with_tolerance(t, q, 0.3, config); + REQUIRE(result.mu > 0.0); + REQUIRE(std::isfinite(result.max_error)); +} + +TEST_CASE("Smoothing spline with tolerance - with boundary conditions", "[smoothing_search]") { + std::vector t, q; + make_noisy_sine(t, q, 20, 0.1); + + SplineConfig config; + config.max_iterations = 20; + config.v0 = 1.0; + config.vn = -1.0; + + auto result = smoothing_spline_with_tolerance(t, q, 0.2, config); + REQUIRE(result.mu > 0.0); + REQUIRE(std::isfinite(result.max_error)); +} + +TEST_CASE("Smoothing spline with tolerance - result spline is usable", "[smoothing_search]") { + std::vector t, q; + make_noisy_sine(t, q, 25, 0.1); + + SplineConfig config; + config.max_iterations = 20; + + auto result = smoothing_spline_with_tolerance(t, q, 0.15, config); + + // The returned spline should be evaluable + for (double ti = t.front(); ti <= t.back(); ti += 0.1) { + REQUIRE(std::isfinite(result.spline.evaluate(ti))); + REQUIRE(std::isfinite(result.spline.evaluate_velocity(ti))); + REQUIRE(std::isfinite(result.spline.evaluate_acceleration(ti))); + } +} diff --git a/cpp/tests/test_trapezoidal_trajectory.cpp b/cpp/tests/test_trapezoidal_trajectory.cpp new file mode 100644 index 0000000..e3ca824 --- /dev/null +++ b/cpp/tests/test_trapezoidal_trajectory.cpp @@ -0,0 +1,115 @@ +#include +#include + +#include +#include "test_data.hpp" + +#include + +using namespace interpolatecpp::motion; +using namespace interpolatecpp::test; +using Catch::Matchers::WithinAbs; + +TEST_CASE("TrapezoidalTrajectory velocity-based", "[trapezoidal]") { + SECTION("Basic generation") { + TrapezoidalTrajectory traj(0.0, 10.0, 2.0, 3.0); + REQUIRE(traj.duration() > 0.0); + + auto r0 = traj.evaluate(traj.t_start()); + auto rT = traj.evaluate(traj.t_end()); + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(rT.position, WithinAbs(10.0, kNumericalAtol)); + } + + SECTION("With initial/final velocities") { + TrapezoidalTrajectory traj(1.0, 8.0, 2.0, 3.0, 0.5, 1.0); + REQUIRE(traj.duration() > 0.0); + + auto r0 = traj.evaluate(traj.t_start()); + REQUIRE_THAT(r0.position, WithinAbs(1.0, kNumericalAtol)); + } + + SECTION("Velocity constraints") { + TrapezoidalTrajectory traj(0.0, 20.0, 2.0, 3.0); + double T = traj.duration(); + + for (int i = 0; i <= 100; ++i) { + double t = traj.t_start() + T * i / 100.0; + auto r = traj.evaluate(t); + REQUIRE(std::abs(r.velocity) <= 3.0 + kNumericalAtol); + } + } + + SECTION("Acceleration constraints") { + TrapezoidalTrajectory traj(0.0, 15.0, 1.5, 3.0); + + for (int i = 0; i <= 100; ++i) { + double t = traj.t_start() + traj.duration() * i / 100.0; + auto r = traj.evaluate(t); + REQUIRE(std::abs(r.acceleration) <= 1.5 + kNumericalAtol); + } + } +} + +TEST_CASE("TrapezoidalTrajectory duration-based", "[trapezoidal]") { + SECTION("Fixed duration") { + TrapezoidalTrajectory traj(TrapezoidalTrajectory::DurationBased{}, + 0.0, 10.0, 2.0, 0.0, 0.0, 0.0, 8.0); + REQUIRE_THAT(traj.duration(), WithinAbs(8.0, kNumericalAtol)); + + auto r0 = traj.evaluate(0.0); + auto rT = traj.evaluate(8.0); + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + REQUIRE_THAT(rT.position, WithinAbs(10.0, kNumericalAtol)); + } +} + +TEST_CASE("TrapezoidalTrajectory edge cases", "[trapezoidal]") { + SECTION("Zero displacement") { + TrapezoidalTrajectory traj(5.0, 5.0, 2.0, 3.0); + auto r = traj.evaluate(0.0); + REQUIRE_THAT(r.position, WithinAbs(5.0, kNumericalAtol)); + } + + SECTION("Negative displacement") { + TrapezoidalTrajectory traj(10.0, 0.0, 2.0, 3.0); + auto rT = traj.evaluate(traj.t_end()); + REQUIRE_THAT(rT.position, WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("High constraints") { + TrapezoidalTrajectory traj(0.0, 1.0, 1000.0, 1000.0); + REQUIRE(traj.duration() > 0.0); + } +} + +TEST_CASE("TrapezoidalTrajectory heuristic velocities", "[trapezoidal]") { + std::vector pts = {0, 3, 8, 12}; + std::vector times = {0, 1, 2, 3}; + auto vels = TrapezoidalTrajectory::heuristic_velocities(pts, times, 4.0); + + REQUIRE(vels.size() == 4); + for (double v : vels) { + REQUIRE(std::abs(v) <= 4.0 + kNumericalAtol); + } +} + +TEST_CASE("TrapezoidalTrajectory waypoint interpolation", "[trapezoidal]") { + SECTION("Basic interpolation") { + std::vector pts = {0, 5, 10}; + auto segments = + TrapezoidalTrajectory::interpolate_waypoints(pts, 2.0, 3.0); + + REQUIRE(segments.size() == 2); + + auto r0 = TrapezoidalTrajectory::evaluate_multipoint(segments, 0.0); + REQUIRE_THAT(r0.position, WithinAbs(0.0, kNumericalAtol)); + } + + SECTION("Minimum points validation") { + std::vector pts = {5.0}; + REQUIRE_THROWS_AS( + TrapezoidalTrajectory::interpolate_waypoints(pts, 2.0, 3.0), + std::invalid_argument); + } +} diff --git a/cpp/tests/test_tridiagonal.cpp b/cpp/tests/test_tridiagonal.cpp new file mode 100644 index 0000000..aa1fa03 --- /dev/null +++ b/cpp/tests/test_tridiagonal.cpp @@ -0,0 +1,103 @@ +#include +#include + +#include + +#include "test_data.hpp" + +using namespace interpolatecpp; +using namespace interpolatecpp::test; + +TEST_CASE("Tridiagonal solver", "[tridiagonal]") { + SECTION("4x4 known-answer test") { + // System from Python docstring example: + // a = [0, 1, 2, 3], b = [2, 3, 4, 5], c = [1, 2, 3, 0], d = [1, 2, 3, 4] + Eigen::VectorXd lower = (Eigen::VectorXd(4) << 0, 1, 2, 3).finished(); + Eigen::VectorXd main = (Eigen::VectorXd(4) << 2, 3, 4, 5).finished(); + Eigen::VectorXd upper = (Eigen::VectorXd(4) << 1, 2, 3, 0).finished(); + Eigen::VectorXd rhs = (Eigen::VectorXd(4) << 1, 2, 3, 4).finished(); + + auto x = solve_tridiagonal(lower, main, upper, rhs); + + // Verify: A*x should equal d + // Manually reconstruct A*x for verification + Eigen::VectorXd ax(4); + ax(0) = main(0) * x(0) + upper(0) * x(1); + ax(1) = lower(1) * x(0) + main(1) * x(1) + upper(1) * x(2); + ax(2) = lower(2) * x(1) + main(2) * x(2) + upper(2) * x(3); + ax(3) = lower(3) * x(2) + main(3) * x(3); + + for (int i = 0; i < 4; ++i) { + REQUIRE_THAT(ax(i), Catch::Matchers::WithinAbs(rhs(i), kRegularAtol)); + } + } + + SECTION("Identity-like system (diagonal = 1)") { + int n = 5; + Eigen::VectorXd lower = Eigen::VectorXd::Zero(n); + Eigen::VectorXd main = Eigen::VectorXd::Ones(n); + Eigen::VectorXd upper = Eigen::VectorXd::Zero(n); + Eigen::VectorXd rhs = (Eigen::VectorXd(n) << 1, 2, 3, 4, 5).finished(); + + auto x = solve_tridiagonal(lower, main, upper, rhs); + + for (int i = 0; i < n; ++i) { + REQUIRE_THAT(x(i), Catch::Matchers::WithinAbs(rhs(i), kRegularAtol)); + } + } + + SECTION("2x2 system") { + Eigen::VectorXd lower = (Eigen::VectorXd(2) << 0, 1).finished(); + Eigen::VectorXd main = (Eigen::VectorXd(2) << 2, 3).finished(); + Eigen::VectorXd upper = (Eigen::VectorXd(2) << 1, 0).finished(); + Eigen::VectorXd rhs = (Eigen::VectorXd(2) << 5, 7).finished(); + + auto x = solve_tridiagonal(lower, main, upper, rhs); + + // Verify: [2 1; 1 3] * x = [5; 7] + REQUIRE_THAT(main(0) * x(0) + upper(0) * x(1), + Catch::Matchers::WithinAbs(rhs(0), kRegularAtol)); + REQUIRE_THAT(lower(1) * x(0) + main(1) * x(1), + Catch::Matchers::WithinAbs(rhs(1), kRegularAtol)); + } + + SECTION("1x1 system") { + Eigen::VectorXd lower = (Eigen::VectorXd(1) << 0).finished(); + Eigen::VectorXd main = (Eigen::VectorXd(1) << 4).finished(); + Eigen::VectorXd upper = (Eigen::VectorXd(1) << 0).finished(); + Eigen::VectorXd rhs = (Eigen::VectorXd(1) << 8).finished(); + + auto x = solve_tridiagonal(lower, main, upper, rhs); + + REQUIRE_THAT(x(0), Catch::Matchers::WithinAbs(2.0, kRegularAtol)); + } + + SECTION("Throws on zero pivot") { + Eigen::VectorXd lower = (Eigen::VectorXd(2) << 0, 1).finished(); + Eigen::VectorXd main = (Eigen::VectorXd(2) << 0, 3).finished(); // zero pivot + Eigen::VectorXd upper = (Eigen::VectorXd(2) << 1, 0).finished(); + Eigen::VectorXd rhs = (Eigen::VectorXd(2) << 5, 7).finished(); + + REQUIRE_THROWS_AS(solve_tridiagonal(lower, main, upper, rhs), std::invalid_argument); + } + + SECTION("Symmetric positive definite system") { + // SPD tridiagonal: main = [4,4,4], off-diag = [1,1] + Eigen::VectorXd lower = (Eigen::VectorXd(3) << 0, 1, 1).finished(); + Eigen::VectorXd main = (Eigen::VectorXd(3) << 4, 4, 4).finished(); + Eigen::VectorXd upper = (Eigen::VectorXd(3) << 1, 1, 0).finished(); + Eigen::VectorXd rhs = (Eigen::VectorXd(3) << 6, 12, 10).finished(); + + auto x = solve_tridiagonal(lower, main, upper, rhs); + + // Verify A*x = rhs + Eigen::VectorXd ax(3); + ax(0) = main(0) * x(0) + upper(0) * x(1); + ax(1) = lower(1) * x(0) + main(1) * x(1) + upper(1) * x(2); + ax(2) = lower(2) * x(1) + main(2) * x(2); + + for (int i = 0; i < 3; ++i) { + REQUIRE_THAT(ax(i), Catch::Matchers::WithinAbs(rhs(i), kRegularAtol)); + } + } +} diff --git a/docs/api-reference.md b/docs/api-reference.md index 60f6207..53d6d0a 100644 --- a/docs/api-reference.md +++ b/docs/api-reference.md @@ -150,6 +150,33 @@ velocity = bspline.evaluate_derivative(t, order=1) options: show_source: false +#### CubicBSplineInterpolation {#cubic-b-spline-interpolation} + +::: interpolatepy.CubicBSplineInterpolation + options: + show_source: false + +**Example:** +```python +from interpolatepy import CubicBSplineInterpolation +import numpy as np + +# 2D data points +points = np.array([[0, 0], [1, 2], [3, 1], [5, 3], [7, 0]]) + +# Create cubic B-spline interpolation with chord-length parameterization +bspline = CubicBSplineInterpolation( + points=points, + method="chord_length", + auto_derivatives=True +) + +# Evaluate curve at parameter value +u = 0.5 +position = bspline.evaluate(u) +derivative = bspline.evaluate_derivative(u, order=1) +``` + ## Motion Profiles ### DoubleSTrajectory {#double-s-trajectory} @@ -238,6 +265,18 @@ pos, vel, acc = traj_func(t) print(f"At t={t}: pos={pos:.2f}, vel={vel:.2f}, acc={acc:.2f}") ``` +#### CalculationParams + +::: interpolatepy.CalculationParams + options: + show_source: false + +#### InterpolationParams + +::: interpolatepy.InterpolationParams + options: + show_source: false + ### PolynomialTrajectory {#polynomial-trajectory} ::: interpolatepy.PolynomialTrajectory @@ -291,6 +330,27 @@ pos, vel, acc, jerk = traj_func(t) options: show_source: false +**Example:** +```python +from interpolatepy import ParabolicBlendTrajectory +import numpy as np + +# Define via points and timing +q = [0.0, 5.0, 2.0, 8.0] # Via positions +t = [0.0, 2.0, 4.0, 6.0] # Nominal times +dt_blend = [0.5, 0.5, 0.5, 0.5] # Blend durations at each via point + +# Generate trajectory +traj = ParabolicBlendTrajectory(q, t, dt_blend) +traj_func, duration = traj.generate() + +# Evaluate +t_eval = np.linspace(0, duration, 200) +results = [traj_func(ti) for ti in t_eval] +positions = [r[0] for r in results] +velocities = [r[1] for r in results] +``` + ## Quaternion Interpolation ### Quaternion @@ -373,8 +433,39 @@ angular_acceleration = squad.evaluate_acceleration(t) options: show_source: false - - +### LogQuaternionInterpolation {#log-quaternion-interpolation} + +::: interpolatepy.LogQuaternionInterpolation + options: + show_source: false + +**Example:** +```python +from interpolatepy import LogQuaternionInterpolation, Quaternion +import numpy as np + +# Define rotation waypoints +times = [0.0, 1.0, 2.0, 3.0] +quaternions = [ + Quaternion.identity(), + Quaternion.from_angle_axis(np.pi / 4, np.array([1, 0, 0])), + Quaternion.from_angle_axis(np.pi / 2, np.array([0, 1, 0])), + Quaternion.from_angle_axis(np.pi / 3, np.array([0, 0, 1])), +] + +# Interpolate in logarithmic quaternion space +log_interp = LogQuaternionInterpolation(times, quaternions) + +# Evaluate smooth rotation +q = log_interp.evaluate(1.5) +omega = log_interp.evaluate_velocity(1.5) +``` + +### ModifiedLogQuaternionInterpolation {#modified-log-quaternion-interpolation} + +::: interpolatepy.ModifiedLogQuaternionInterpolation + options: + show_source: false ## Path Planning @@ -629,9 +720,78 @@ def validate_waypoints(t_points, q_points): raise ValueError("Time points must be strictly increasing") ``` -## Version Information +## Protocols + +InterpolatePy defines [PEP 544](https://peps.python.org/pep-0544/) runtime-checkable protocols for structural typing. Classes conform by having the right method signatures โ€” no inheritance required. + +### ScalarTrajectory + +::: interpolatepy.ScalarTrajectory + options: + show_source: false + +**Conforming classes:** `CubicSpline`, `CubicSplineWithAcceleration1`, `CubicSplineWithAcceleration2`, `CubicSmoothingSpline`, `DoubleSTrajectory` + +### CurveEvaluator + +::: interpolatepy.CurveEvaluator + options: + show_source: false + +**Conforming classes:** `BSpline`, `BSplineInterpolator`, `CubicBSplineInterpolation`, `SmoothingCubicBSpline`, `ApproximationBSpline` + +### GeometricPath + +::: interpolatepy.GeometricPath + options: + show_source: false + +**Conforming classes:** `LinearPath`, `CircularPath` + +### QuaternionTrajectory + +::: interpolatepy.QuaternionTrajectory + options: + show_source: false -Current version: **2.0.0** +**Conforming classes:** `SquadC2`, `LogQuaternionInterpolation`, `ModifiedLogQuaternionInterpolation`, `QuaternionSpline` + +### TrajectoryFunction + +::: interpolatepy.TrajectoryFunction + options: + show_source: false + +**Conforming callables:** Output of `TrapezoidalTrajectory`, `ParabolicBlendTrajectory`, and `PolynomialTrajectory` factory methods. + +**Example:** +```python +from interpolatepy import ScalarTrajectory, CubicSpline + +# Check protocol conformance at runtime +spline = CubicSpline([0, 1, 2], [0, 1, 0]) +print(isinstance(spline, ScalarTrajectory)) # True +``` + +## Backend + +### HAS_CPP + +`interpolatepy.HAS_CPP: bool` + +Runtime flag indicating whether the compiled C++ extension is active. + +- `True`: C++ backend loaded; computation-heavy classes use native code +- `False`: Pure-Python fallback; all algorithms still work, using NumPy + +```python +import interpolatepy +print(f"C++ backend: {interpolatepy.HAS_CPP}") +``` + +Set `INTERPOLATEPY_NO_CPP=1` to force `False`. See the [Architecture Guide](architecture.md) for details. + +## Version Information Access version information: ```python diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..5c5fb68 --- /dev/null +++ b/docs/architecture.md @@ -0,0 +1,122 @@ +# Architecture + +InterpolatePy uses a **dual-backend architecture**: a compiled C++ extension for performance-critical workloads, with an automatic pure-Python fallback when the extension is unavailable. + +## Overview + +```mermaid +graph TD + User["User Code"] --> Init["interpolatepy/__init__.py"] + Init --> API["_api.py
(backend router)"] + API --> Check{"HAS_CPP?"} + Check -->|Yes| Adapters["_adapters/
(C++ wrappers)"] + Check -->|No| Python["Pure-Python modules
(cubic_spline.py, etc.)"] + Adapters --> CPP["interpolatecpp_py.so
(pybind11 extension)"] + Init --> PureOnly["Always pure-Python:
Quaternion, protocols,
TrajectoryParams, etc."] +``` + +## Backend Detection + +The module `_backend.py` handles backend detection at import time: + +1. Checks for the `INTERPOLATEPY_NO_CPP` environment variable +2. If not set, attempts to import the compiled extension `interpolatecpp_py` +3. Sets `HAS_CPP = True` on success, `False` on `ImportError` + +```python +import interpolatepy +print(f"C++ backend active: {interpolatepy.HAS_CPP}") +``` + +To force pure-Python mode: +```bash +export INTERPOLATEPY_NO_CPP=1 +``` + +## Import Routing + +`_api.py` uses `HAS_CPP` to decide where each symbol comes from: + +| `HAS_CPP` | Import source | Example | +|-----------|--------------|---------| +| `True` | `_adapters._spline.CubicSpline` | C++ core + Python `plot()` | +| `False` | `cubic_spline.CubicSpline` | Pure Python implementation | + +The user-facing API (`__init__.py`) is identical regardless of backend. Code that imports from `interpolatepy` works the same either way. + +## Adapter Pattern + +Each adapter in `_adapters/` subclasses the pybind11-exposed C++ class and adds Python-only convenience methods: + +``` +_adapters/ + _spline.py CubicSpline, CubicSmoothingSpline, ... + _bspline.py BSpline, BSplineInterpolator, ... + _motion.py DoubleSTrajectory, TrapezoidalTrajectory, ... + _paths.py LinearPath, CircularPath + _quaternion.py SquadC2, QuaternionSpline, ... + _direct.py Direct re-exports (StateParams, TrajectoryBounds, ...) +``` + +**What adapters add:** + +- `plot()` methods (matplotlib visualization) +- `__repr__` for readable string representation +- API normalization (e.g., C++ returns `FullTrajectoryResult` structs, adapters unpack to tuples) + +**What stays pure-Python:** + +- `Quaternion` class (`quat_core.py`) -- core quaternion math +- Protocol definitions (`protocols.py`) -- structural typing interfaces +- Parameter dataclasses (`TrajectoryParams`, `CalculationParams`, `InterpolationParams`) + +## C++ Library Structure + +The C++ implementation lives in the `cpp/` directory: + +``` +cpp/ + include/interpolatecpp/ Header-only public interface + spline/ Cubic spline family (5 headers) + bspline/ B-spline family (6 headers) + motion/ Motion profiles (5 headers) + path/ Path primitives (4 headers) + quat/ Quaternion interpolation (5 headers) + concepts.hpp C++20 concepts for type safety + tridiagonal.hpp Shared tridiagonal solver + src/ Implementation files (23 .cpp files) + bindings/ pybind11 binding definitions + tests/ Catch2 unit tests (142 tests) + examples/ C++ usage examples (16 programs) + CMakeLists.txt Build configuration +``` + +**Key design choices:** + +- **C++20** with concepts for compile-time type checking +- **Eigen 3.4** for linear algebra (fetched automatically via CMake FetchContent) +- **pybind11** for Python bindings (also fetched via FetchContent) +- **Catch2** for C++ unit testing +- **Header-only public interface** with separate compilation units + +## Building the C++ Extension + +See the [Installation Guide](installation.md#c-backend-optional) for build instructions. + +### CMake Options + +| Option | Default | Description | +|--------|---------|-------------| +| `INTERPOLATECPP_BUILD_TESTS` | `ON` | Build Catch2 unit tests | +| `INTERPOLATECPP_BUILD_BINDINGS` | `OFF` | Build pybind11 Python bindings | +| `INTERPOLATECPP_BUILD_EXAMPLES` | `OFF` | Build C++ example programs | + +## Performance Characteristics + +The C++ backend provides the most benefit for: + +- **Spline construction** -- solving tridiagonal systems, computing coefficients +- **Batch evaluation** -- evaluating trajectories at many time points +- **Motion profile planning** -- iterative phase calculations in DoubleSTrajectory + +The pure-Python backend uses vectorized NumPy operations and remains fast for most use cases. The C++ backend is an optimization, not a requirement. diff --git a/docs/changelog.md b/docs/changelog.md index 19fff51..c7594ee 100644 --- a/docs/changelog.md +++ b/docs/changelog.md @@ -5,7 +5,27 @@ All notable changes to InterpolatePy will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). -## [2.0.0] - 2025-01-XX +## [3.0.0] - 2025-08-14 + +### Added +- **C++ backend** with pybind11 bindings for performance-critical algorithms + - Eigen-based C++20 implementation of all spline, B-spline, motion profile, path, and quaternion algorithms + - Transparent adapter layer (`_adapters/`) preserving the existing Python API + - Runtime backend detection via `HAS_CPP` flag + - Pure-Python fallback when C++ extension is unavailable + - `INTERPOLATEPY_NO_CPP=1` environment variable to force pure-Python mode + - 142 C++ unit tests with Catch2 framework + - 16 C++ example programs +- **Protocol definitions** (`protocols.py`) for structural typing + - `ScalarTrajectory`, `CurveEvaluator`, `GeometricPath`, `QuaternionTrajectory`, `TrajectoryFunction` +- **Duration-based `TrapezoidalTrajectory` constructor** + +### Changed +- Minimum Python version updated to 3.11 +- Updated dependencies: NumPy >=2.3.0, SciPy >=1.16.0, Matplotlib >=3.10.5 +- Backend-aware import system via `_api.py` conditionally routes to C++ or pure-Python + +## [2.0.0] - 2025-08-06 ### Added - **Complete MkDocs documentation** with Material theme @@ -58,10 +78,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Breaking Changes - Some internal API changes for consistency -- Updated minimum Python version to 3.10 +- Updated minimum Python version to 3.11 - Renamed some utility functions for clarity -## [1.5.0] - 2024-XX-XX +## [1.1.0] - 2025-05-17 ### Added - Initial quaternion interpolation support @@ -76,7 +96,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Edge cases in cubic spline interpolation - Performance issues with large datasets -## [1.0.0] - 2024-XX-XX +## [1.0.1] - 2025-03-26 ### Added - Initial release of InterpolatePy @@ -172,19 +192,23 @@ result = spline.evaluate(t) ## Roadmap +### Completed + +- [x] **C++ Backend**: Native C++20 implementation with pybind11 bindings (v3.0.0) +- [x] **Protocol Definitions**: Structural typing for algorithm interchangeability (v3.0.0) + ### Planned Features -#### Version 2.1 (Next Minor Release) +#### Version 3.1 (Next Minor Release) - [ ] **Enhanced B-splines**: NURBS support and surface interpolation - [ ] **Optimization Integration**: Interface with scipy.optimize for trajectory optimization -- [ ] **Real-time Interface**: Zero-copy evaluation for embedded systems - [ ] **Extended Examples**: More industry-specific use cases +- [ ] **Pre-built Wheels**: Distribute pre-compiled C++ extensions via PyPI -#### Version 2.2 +#### Version 3.2 - [ ] **Multi-dimensional Trajectories**: Native support for N-dimensional spaces - [ ] **Constraint Handling**: General inequality constraints on derivatives - [ ] **Adaptive Algorithms**: Automatic degree/parameter selection -- [ ] **GPU Acceleration**: CUDA support for large-scale problems #### Version 3.0 (Future Major Release) - [ ] **Modern Python Features**: Python 3.12+ features and optimizations diff --git a/docs/contributing.md b/docs/contributing.md index 0038037..ec8a16e 100644 --- a/docs/contributing.md +++ b/docs/contributing.md @@ -48,20 +48,39 @@ Thank you for your interest in contributing to InterpolatePy! This guide will he ``` InterpolatePy/ -โ”œโ”€โ”€ interpolatepy/ # Main package source -โ”‚ โ”œโ”€โ”€ __init__.py # Package exports -โ”‚ โ”œโ”€โ”€ cubic_spline.py # Core algorithms -โ”‚ โ”œโ”€โ”€ double_s.py # Motion profiles -โ”‚ โ””โ”€โ”€ ... # Other modules -โ”œโ”€โ”€ tests/ # Test suite -โ”‚ โ”œโ”€โ”€ test_splines.py # Algorithm tests -โ”‚ โ””โ”€โ”€ ... # Other test files -โ”œโ”€โ”€ examples/ # Usage examples -โ”œโ”€โ”€ docs/ # Documentation source -โ”œโ”€โ”€ pyproject.toml # Build configuration -โ””โ”€โ”€ README.md # Project overview +โ”œโ”€โ”€ interpolatepy/ # Main Python package +โ”‚ โ”œโ”€โ”€ __init__.py # Public API exports +โ”‚ โ”œโ”€โ”€ _api.py # Backend-aware import router +โ”‚ โ”œโ”€โ”€ _backend.py # C++ extension detection +โ”‚ โ”œโ”€โ”€ _adapters/ # C++-backed class adapters +โ”‚ โ”‚ โ”œโ”€โ”€ _spline.py # Spline adapters +โ”‚ โ”‚ โ”œโ”€โ”€ _bspline.py # B-spline adapters +โ”‚ โ”‚ โ”œโ”€โ”€ _motion.py # Motion profile adapters +โ”‚ โ”‚ โ”œโ”€โ”€ _paths.py # Path adapters +โ”‚ โ”‚ โ”œโ”€โ”€ _quaternion.py # Quaternion adapters +โ”‚ โ”‚ โ””โ”€โ”€ _direct.py # Direct C++ re-exports +โ”‚ โ”œโ”€โ”€ protocols.py # PEP 544 protocol definitions +โ”‚ โ”œโ”€โ”€ cubic_spline.py # Pure-Python algorithms +โ”‚ โ”œโ”€โ”€ double_s.py # S-curve motion profiles +โ”‚ โ”œโ”€โ”€ quat_core.py # Quaternion class +โ”‚ โ””โ”€โ”€ ... # Other algorithm modules +โ”œโ”€โ”€ cpp/ # C++ backend (optional) +โ”‚ โ”œโ”€โ”€ include/interpolatecpp/ # Header-only public interface +โ”‚ โ”œโ”€โ”€ src/ # Implementation files (23 .cpp) +โ”‚ โ”œโ”€โ”€ bindings/ # pybind11 binding definitions +โ”‚ โ”œโ”€โ”€ tests/ # Catch2 unit tests +โ”‚ โ”œโ”€โ”€ examples/ # C++ usage examples +โ”‚ โ””โ”€โ”€ CMakeLists.txt # CMake build configuration +โ”œโ”€โ”€ tests/ # Python test suite +โ”œโ”€โ”€ examples/ # Python usage examples (23 scripts) +โ”œโ”€โ”€ docs/ # MkDocs documentation source +โ”œโ”€โ”€ pyproject.toml # Build configuration +โ”œโ”€โ”€ mkdocs.yml # Documentation site config +โ””โ”€โ”€ README.md # Project overview ``` +See the [Architecture Guide](architecture.md) for details on the dual-backend design. + ## Development Workflow ### 1. Create a Branch @@ -508,6 +527,39 @@ def create_robot_trajectory(): return trajectories ``` +## C++ Development + +### Building and Testing C++ + +```bash +cd cpp +mkdir build && cd build +cmake .. -DINTERPOLATECPP_BUILD_TESTS=ON -DINTERPOLATECPP_BUILD_BINDINGS=ON +make -j$(nproc) + +# Run C++ unit tests +./tests/interpolatecpp_tests + +# Run specific test +./tests/interpolatecpp_tests "[cubic_spline]" +``` + +### Adding a New C++ Algorithm + +1. **Header**: Create `include/interpolatecpp//new_algo.hpp` +2. **Source**: Create `src/new_algo.cpp` +3. **Binding**: Add to `bindings/bind_.cpp` +4. **Adapter**: Create or update `interpolatepy/_adapters/_.py` to subclass the C++ class and add `plot()`, `__repr__` +5. **Wire up**: Add to `_adapters/__init__.py` and `_api.py` (both the `HAS_CPP` and fallback branches) +6. **Tests**: Add Catch2 tests in `tests/` and Python tests in the main `tests/` directory + +### C++ Code Style + +- C++20 with concepts for type safety +- Eigen 3.4 for linear algebra +- `snake_case` for functions, `PascalCase` for classes +- Header-only public interface with separate `.cpp` compilation units + ## Algorithm Contributions ### Adding New Algorithms @@ -516,7 +568,7 @@ When contributing new interpolation algorithms: 1. **Research Background**: Include references to papers/books 2. **Mathematical Foundation**: Document the theory clearly -3. **Implementation**: Follow our class structure guidelines +3. **Implementation**: Follow our class structure guidelines (Python and optionally C++) 4. **Testing**: Comprehensive test coverage 5. **Documentation**: Examples and usage guidelines 6. **Performance**: Benchmarks and complexity analysis diff --git a/docs/index.md b/docs/index.md index 1698dda..070a615 100644 --- a/docs/index.md +++ b/docs/index.md @@ -3,7 +3,7 @@ [![PyPI Downloads](https://static.pepy.tech/badge/interpolatepy)](https://pepy.tech/projects/interpolatepy) [![CI Tests](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/test.yml/badge.svg)](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/test.yml) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) -[![Python](https://img.shields.io/badge/python-3.10+-blue)](https://www.python.org/downloads/) +[![Python](https://img.shields.io/badge/python-3.11+-blue)](https://www.python.org/downloads/) **Production-ready trajectory planning and interpolation for robotics, animation, and scientific computing.** @@ -14,7 +14,7 @@ InterpolatePy provides 20+ algorithms for smooth trajectory generation with prec

โšก Fast Performance

-

Vectorized NumPy operations with ~1ms evaluation for 1000-point cubic splines. Optimized algorithms for real-time applications.

+

Optional C++ backend (Eigen + pybind11) for maximum speed; pure-Python fallback uses vectorized NumPy. ~1ms for 1000-point cubic splines.

๐ŸŽฏ Research-Grade Precision

@@ -152,11 +152,15 @@ plt.show() ## ๐Ÿ› ๏ธ Development & Quality -- **Modern Python**: 3.10+ with strict typing and dataclass-based APIs -- **High Test Coverage**: 85%+ coverage with continuous integration +- **Modern Python**: 3.11+ with strict typing and dataclass-based APIs +- **Dual Backend**: Optional C++ extension for performance; pure-Python fallback always available +- **High Test Coverage**: 85%+ Python coverage + 142 C++ unit tests - **Research-Grade**: Peer-reviewed algorithms from robotics literature - **Production-Ready**: Used in industrial applications and research projects +!!! info "C++ Backend" + InterpolatePy automatically uses a compiled C++ extension when available. Check with `interpolatepy.HAS_CPP`. See the [Architecture Guide](architecture.md) for details. + ## ๐Ÿ“– Getting Started 1. **[Installation Guide](installation.md)** - Get up and running quickly diff --git a/docs/installation.md b/docs/installation.md index c8a681f..2c3e906 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -1,13 +1,13 @@ # Installation Guide -InterpolatePy is available on PyPI and can be installed with pip. We support Python 3.10+ on Windows, macOS, and Linux. +InterpolatePy is available on PyPI and can be installed with pip. We support Python 3.11+ on Windows, macOS, and Linux. ## Requirements -- **Python**: โ‰ฅ3.10 -- **NumPy**: โ‰ฅ2.0.0 -- **SciPy**: โ‰ฅ1.15.2 -- **Matplotlib**: โ‰ฅ3.10.1 +- **Python**: โ‰ฅ3.11 +- **NumPy**: โ‰ฅ2.3.0 +- **SciPy**: โ‰ฅ1.16.0 +- **Matplotlib**: โ‰ฅ3.10.5 ## Installation @@ -16,7 +16,7 @@ InterpolatePy is available on PyPI and can be installed with pip. We support Pyt **Step 1: Verify Python Version** ```bash python --version -# Should show Python 3.10 or higher +# Should show Python 3.11 or higher ``` **Step 2: Install InterpolatePy** @@ -101,19 +101,92 @@ python -m pytest tests/ python -m pytest tests/ --cov=interpolatepy --cov-report=html --cov-report=term ``` +## C++ Backend (Optional) + +InterpolatePy includes an optional C++ extension that accelerates computation-heavy algorithms. The pure-Python install works out of the box without it. + +### Check Backend Status + +```python +import interpolatepy +print(f"C++ backend active: {interpolatepy.HAS_CPP}") +``` + +### Building from Source + +**Requirements:** + +- CMake >= 3.21 +- C++20 compatible compiler (GCC >= 10, Clang >= 13, MSVC >= 19.29) +- Python development headers + +Dependencies (Eigen, pybind11, Catch2) are fetched automatically via CMake FetchContent. + +**Build steps:** + +```bash +git clone https://github.com/GiorgioMedico/InterpolatePy.git +cd InterpolatePy/cpp +mkdir build && cd build +cmake .. -DINTERPOLATECPP_BUILD_BINDINGS=ON +make -j$(nproc) +``` + +Copy the compiled extension into the package directory: +```bash +cp bindings/interpolatecpp_py*.so ../../interpolatepy/ +``` + +**Verify:** +```bash +python -c "import interpolatepy; print(interpolatepy.HAS_CPP)" +# Expected: True +``` + +### Disabling C++ Backend + +To force pure-Python mode even when the extension is installed: +```bash +export INTERPOLATEPY_NO_CPP=1 +``` + +### C++ Build Troubleshooting + +**CMake not found or too old:** +```bash +pip install cmake # or: sudo apt install cmake +cmake --version # Must be >= 3.21 +``` + +**Missing C++20 support:** +```bash +g++ --version # Must be >= 10 for C++20 +# On Ubuntu: sudo apt install g++-12 +``` + +**Eigen fetch fails (network issues):** +```bash +# Pre-install Eigen system-wide +sudo apt install libeigen3-dev # Ubuntu/Debian +brew install eigen # macOS +``` + +For more details on the dual-backend architecture, see the [Architecture Guide](architecture.md). + ## Optional Dependencies ### Testing Dependencies -- `pytest>=7.3.1` - Test framework +- `pytest>=8.4.0` - Test framework - `pytest-cov>=4.1.0` - Coverage reporting - `pytest-benchmark>=4.0.0` - Performance benchmarking - `codecov>=2.1.13` - Coverage upload +- `pre-commit>=4.2.0` - Git hooks -### Development Dependencies -- `ruff>=0.1.5` - Linting and formatting -- `mypy>=1.6.1` - Type checking -- `pre-commit>=4.1.0` - Git hooks -- `pyright>=1.1.335` - Additional type checking +### Development Dependencies +- `ruff>=0.12.8` - Linting and formatting +- `mypy>=1.17.0` - Type checking +- `pre-commit>=4.2.0` - Git hooks +- `pyright>=1.1.400` - Additional type checking - `build>=1.0.3` - Package building - `twine>=4.0.2` - Package publishing @@ -196,7 +269,7 @@ pip install InterpolatePy **Solution 2**: Install dependencies manually: ```bash -pip install numpy>=2.0.0 scipy>=1.15.2 matplotlib>=3.10.1 +pip install numpy>=2.3.0 scipy>=1.16.0 matplotlib>=3.10.5 pip install InterpolatePy ``` diff --git a/docs/quickstart.md b/docs/quickstart.md index 4b6ff8f..98cf40f 100644 --- a/docs/quickstart.md +++ b/docs/quickstart.md @@ -153,36 +153,33 @@ plt.show() ### 4. Polynomial Trajectories -For precise boundary condition control: +For precise boundary condition control using static factory methods: ```python -from interpolatepy import PolynomialTrajectory +from interpolatepy import PolynomialTrajectory, BoundaryCondition, TimeInterval import numpy as np +import matplotlib.pyplot as plt -# Define precise boundary conditions (initial and final states) -initial_state = [0.0, 0.0, 0.0, 0.0] # [position, velocity, acceleration, jerk] -final_state = [5.0, 0.0, 0.0, 0.0] # [position, velocity, acceleration, jerk] -total_time = 3.0 - -# Generate 7th-order polynomial trajectory -traj = PolynomialTrajectory( - initial_state=initial_state, - final_state=final_state, - total_time=total_time, - order=7 -) +# Define boundary conditions (position, velocity, acceleration, jerk) +initial = BoundaryCondition(position=0.0, velocity=0.0, acceleration=0.0, jerk=0.0) +final = BoundaryCondition(position=5.0, velocity=0.0, acceleration=0.0, jerk=0.0) +interval = TimeInterval(start=0.0, end=3.0) + +# Generate 7th-order polynomial trajectory (returns a callable) +traj_func = PolynomialTrajectory.order_7_trajectory(initial, final, interval) # Evaluate complete trajectory t_eval = np.linspace(0, 3, 100) -positions = [traj.evaluate(t) for t in t_eval] -velocities = [traj.evaluate_velocity(t) for t in t_eval] -accelerations = [traj.evaluate_acceleration(t) for t in t_eval] -jerks = [traj.evaluate_jerk(t) for t in t_eval] +results = [traj_func(t) for t in t_eval] +positions = [r[0] for r in results] +velocities = [r[1] for r in results] +accelerations = [r[2] for r in results] +jerks = [r[3] for r in results] # Plot all derivatives fig, axes = plt.subplots(4, 1, figsize=(10, 8)) axes[0].plot(t_eval, positions, label='Position') -axes[1].plot(t_eval, velocities, label='Velocity') +axes[1].plot(t_eval, velocities, label='Velocity') axes[2].plot(t_eval, accelerations, label='Acceleration') axes[3].plot(t_eval, jerks, label='Jerk') diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md index c59b647..dd1817c 100644 --- a/docs/troubleshooting.md +++ b/docs/troubleshooting.md @@ -380,6 +380,35 @@ except ValueError as e: print(f"Data validation error: {e}") ``` +## C++ Backend Issues + +### Problem: C++ Extension Not Found + +The C++ backend is optional. If it is not available, the library falls back to pure Python automatically. + +**Check backend status:** +```python +import interpolatepy +print(f"C++ backend: {interpolatepy.HAS_CPP}") +``` + +If `HAS_CPP` is `False` and you want the C++ backend, see the [Installation Guide](installation.md#c-backend-optional) for build instructions. + +### Problem: Force Pure-Python Mode +```bash +export INTERPOLATEPY_NO_CPP=1 +python your_script.py +``` + +### Problem: C++ Build Fails + +Common causes: +- **CMake too old** -- need >= 3.21 +- **Compiler lacks C++20** -- need GCC >= 10, Clang >= 13, or MSVC >= 19.29 +- **Network issues** -- Eigen/pybind11 fetched via CMake FetchContent; install system packages as fallback + +See [C++ Build Troubleshooting](installation.md#c-build-troubleshooting) for detailed solutions. + ## Getting Help If you encounter issues not covered here: @@ -403,7 +432,7 @@ When reporting issues, please include: Example bug report template: ```python # InterpolatePy version: 2.0.0 -# Python version: 3.10.0 +# Python version: 3.11.0 # Platform: Ubuntu 22.04 from interpolatepy import CubicSpline diff --git a/docs/tutorials/path-planning.md b/docs/tutorials/path-planning.md new file mode 100644 index 0000000..421d550 --- /dev/null +++ b/docs/tutorials/path-planning.md @@ -0,0 +1,248 @@ +# Path Planning Tutorial + +This tutorial covers InterpolatePy's geometric path primitives and Frenet frame computation for spatial trajectory planning. + +## Geometric Path Primitives + +InterpolatePy provides two basic geometric paths parameterized by arc length. + +### Linear Path + +A straight line between two 3D points: + +```python +from interpolatepy import LinearPath +import numpy as np + +# Define endpoints +pi = np.array([0, 0, 0]) # Start +pf = np.array([5, 3, 2]) # End + +path = LinearPath(pi, pf) + +# Path properties +print(f"Path length: {path.length:.2f}") +print(f"Tangent (constant): {path.tangent}") + +# Evaluate by arc length parameter s +s = 2.0 # 2 units along the path +position = path.position(s) +velocity = path.velocity(s) # Constant tangent vector +acceleration = path.acceleration(s) # Zero for straight line + +print(f"Position at s={s}: {position}") +``` + +### Circular Path + +A circular arc in 3D space defined by an axis of rotation, a point on the axis, and a starting point: + +```python +from interpolatepy import CircularPath +import numpy as np + +# Define circular arc +r = np.array([0, 0, 1]) # Rotation axis (Z-axis) +d = np.array([0, 0, 0]) # Point on axis (origin) +pi = np.array([1, 0, 0]) # Starting point on circle + +path = CircularPath(r, d, pi) + +# The radius is computed from pi and the axis +print(f"Radius: {path.radius:.2f}") + +# Evaluate at arc length +s = np.pi # Half circle +position = path.position(s) +velocity = path.velocity(s) # Tangent to circle +acceleration = path.acceleration(s) # Centripetal + +print(f"Position at s=pi: {position}") +print(f"Acceleration (centripetal): {acceleration}") +``` + +## Combining Paths with Motion Laws + +Geometric paths define *where* to move. Motion laws define *how fast*. Combine them for complete trajectories: + +```python +from interpolatepy import LinearPath, PolynomialTrajectory, BoundaryCondition, TimeInterval +import numpy as np +import matplotlib.pyplot as plt + +# 1. Define the geometric path +pi = np.array([0, 0, 0]) +pf = np.array([10, 5, 3]) +path = LinearPath(pi, pf) + +# 2. Define the motion law (how arc length varies with time) +# Start and end at rest, traverse the full path length +initial = BoundaryCondition(position=0.0, velocity=0.0) +final = BoundaryCondition(position=path.length, velocity=0.0) +interval = TimeInterval(start=0.0, end=5.0) + +motion_law = PolynomialTrajectory.order_5_trajectory(initial, final, interval) + +# 3. Combine: evaluate motion law to get arc length, then path to get position +t_eval = np.linspace(0, 5, 200) +positions = [] +velocities = [] + +for t in t_eval: + s, ds_dt, _, _ = motion_law(t) # Arc length and its derivatives + pos = path.position(s) # 3D position + vel = path.velocity(s) * ds_dt # Chain rule: dp/dt = dp/ds * ds/dt + positions.append(pos) + velocities.append(np.linalg.norm(vel)) + +positions = np.array(positions) + +# Plot +fig, axes = plt.subplots(1, 2, figsize=(14, 5)) + +# 3D path +ax3d = fig.add_subplot(121, projection="3d") +ax3d.plot(positions[:, 0], positions[:, 1], positions[:, 2], "b-", linewidth=2) +ax3d.scatter(*pi, color="green", s=100, label="Start") +ax3d.scatter(*pf, color="red", s=100, label="End") +ax3d.set_xlabel("X") +ax3d.set_ylabel("Y") +ax3d.set_zlabel("Z") +ax3d.set_title("Linear Path with 5th-Order Motion Law") +ax3d.legend() + +# Speed profile +axes[1].plot(t_eval, velocities, "r-", linewidth=2) +axes[1].set_xlabel("Time (s)") +axes[1].set_ylabel("Speed") +axes[1].set_title("Speed Profile (smooth start/stop)") +axes[1].grid(True) + +plt.tight_layout() +plt.show() +``` + +## Frenet Frame Computation + +The Frenet-Serret frame provides a local coordinate system (tangent, normal, binormal) at each point along a curve. This is essential for tool orientation in CNC machining and robotic path following. + +### Basic Usage + +```python +from interpolatepy import compute_trajectory_frames +from interpolatepy import helicoidal_trajectory_with_derivatives +import numpy as np + +# Define a helicoidal (helical) trajectory +r = 2.0 # Radius +d = 0.5 # Pitch per radian + +def helix_func(u): + return helicoidal_trajectory_with_derivatives(u, r, d) + +# Compute frames at sampled points +u_values = np.linspace(0, 4 * np.pi, 100) +points, frames = compute_trajectory_frames(helix_func, u_values) + +# points: (100, 3) array of 3D positions +# frames: (100, 3, 3) array where frames[i] = [tangent, normal, binormal] + +print(f"Tangent at u=0: {frames[0, 0]}") +print(f"Normal at u=0: {frames[0, 1]}") +print(f"Binormal at u=0: {frames[0, 2]}") +``` + +### Visualizing Frames + +```python +from interpolatepy import compute_trajectory_frames, plot_frames +from interpolatepy import helicoidal_trajectory_with_derivatives +import numpy as np +import matplotlib.pyplot as plt + +r, d = 2.0, 0.5 +u_values = np.linspace(0, 4 * np.pi, 100) + +def helix_func(u): + return helicoidal_trajectory_with_derivatives(u, r, d) + +points, frames = compute_trajectory_frames(helix_func, u_values) + +fig = plt.figure(figsize=(10, 8)) +ax = fig.add_subplot(111, projection="3d") +plot_frames(ax, points, frames, scale=0.5, skip=10) +ax.set_title("Helicoidal Trajectory with Frenet Frames") +ax.set_xlabel("X") +ax.set_ylabel("Y") +ax.set_zlabel("Z") +plt.tight_layout() +plt.show() +``` + +### Tool Orientation + +For CNC machining or robotic applications, you can specify an additional tool orientation relative to the Frenet frame: + +```python +from interpolatepy import compute_trajectory_frames +from interpolatepy import circular_trajectory_with_derivatives +import numpy as np + +def circle_func(u): + return circular_trajectory_with_derivatives(u, radius=3.0) + +u_values = np.linspace(0, 2 * np.pi, 50) + +# Add tool tilt: (roll, pitch, yaw) relative to Frenet frame +points, frames = compute_trajectory_frames( + circle_func, + u_values, + tool_orientation=(0.1, -0.2, 0.0) # Small roll and pitch tilt +) +``` + +### Custom Path Functions + +Any function returning `(position, first_derivative, second_derivative)` works: + +```python +from interpolatepy import compute_trajectory_frames +import numpy as np + +def lissajous_3d(u): + """3D Lissajous curve with derivatives.""" + position = np.array([ + np.sin(2 * u), + np.sin(3 * u), + np.sin(5 * u) * 0.5 + ]) + first_derivative = np.array([ + 2 * np.cos(2 * u), + 3 * np.cos(3 * u), + 5 * np.cos(5 * u) * 0.5 + ]) + second_derivative = np.array([ + -4 * np.sin(2 * u), + -9 * np.sin(3 * u), + -25 * np.sin(5 * u) * 0.5 + ]) + return position, first_derivative, second_derivative + +u_values = np.linspace(0, 2 * np.pi, 200) +points, frames = compute_trajectory_frames(lissajous_3d, u_values) +``` + +## Built-in Trajectory Functions + +InterpolatePy provides ready-made trajectory functions for common curves: + +| Function | Description | +|----------|-------------| +| `helicoidal_trajectory_with_derivatives(u, r, d)` | Helical curve with radius `r` and pitch `d` | +| `circular_trajectory_with_derivatives(u, radius)` | Circle in the XY plane | + +## Next Steps + +- **[API Reference](../api-reference.md#path-planning)** for complete method documentation +- **[Algorithms Guide](../algorithms.md)** for Frenet-Serret mathematical foundations +- **Example scripts**: `examples/simple_paths_ex.py`, `examples/frenet_frame_ex.py` diff --git a/docs/tutorials/quaternion-interpolation.md b/docs/tutorials/quaternion-interpolation.md new file mode 100644 index 0000000..9ff5436 --- /dev/null +++ b/docs/tutorials/quaternion-interpolation.md @@ -0,0 +1,212 @@ +# Quaternion Interpolation Tutorial + +Quaternions provide singularity-free rotation interpolation for 3D orientations. This tutorial covers InterpolatePy's quaternion algorithms, from basic SLERP to Cยฒ-continuous rotation splines. + +## Why Quaternions? + +Euler angles suffer from gimbal lock, and rotation matrices are redundant (9 numbers for 3 DOF). Quaternions are: + +- **Compact**: 4 numbers represent any 3D rotation +- **Singularity-free**: No gimbal lock +- **Smooth interpolation**: SLERP gives constant angular velocity +- **Numerically stable**: Easy to normalize + +## Basic Quaternion Operations + +The `Quaternion` class provides core operations: + +```python +from interpolatepy import Quaternion +import numpy as np + +# Create quaternions +q_identity = Quaternion.identity() # No rotation +q_from_axis = Quaternion.from_angle_axis(np.pi/2, np.array([0, 0, 1])) # 90 deg about Z +q_from_euler = Quaternion.from_euler_angles(0.3, 0.5, 0.1) # roll, pitch, yaw + +# Operations +q_product = q_from_axis * q_from_euler # Compose rotations +q_inverse = q_from_axis.inverse() # Inverse rotation +q_normalized = q_from_axis.unit() # Normalize + +# Convert back +axis, angle = q_from_axis.to_axis_angle() +roll, pitch, yaw = q_from_euler.to_euler_angles() + +# Access components +scalar = q_from_axis.s_ # Scalar part +vector = q_from_axis.v_ # Vector part (numpy array of length 3) +rotation_matrix = q_from_axis.R() # 3x3 rotation matrix +``` + +## SLERP: Two-Point Interpolation + +SLERP (Spherical Linear Interpolation) smoothly interpolates between two orientations at constant angular velocity: + +```python +from interpolatepy import Quaternion +import numpy as np + +# Two orientations +q_start = Quaternion.identity() +q_end = Quaternion.from_angle_axis(np.pi / 2, np.array([0, 0, 1])) + +# Interpolate at parameter t in [0, 1] +q_mid = q_start.slerp(q_end, 0.5) # Halfway rotation (45 deg about Z) +q_quarter = q_start.slerp(q_end, 0.25) # Quarter way + +print(f"Start: {q_start.to_euler_angles()}") +print(f"Quarter: {q_quarter.to_euler_angles()}") +print(f"Mid: {q_mid.to_euler_angles()}") +print(f"End: {q_end.to_euler_angles()}") +``` + +SLERP is ideal for interpolating between exactly two orientations. For multiple waypoints, use one of the spline methods below. + +## QuaternionSpline: Multi-Point Interpolation + +`QuaternionSpline` handles multiple orientation waypoints with automatic method selection: + +```python +from interpolatepy import QuaternionSpline, Quaternion +import numpy as np + +# Define orientation keyframes +times = [0.0, 1.0, 2.0, 3.0, 4.0] +orientations = [ + Quaternion.identity(), + Quaternion.from_euler_angles(0.3, 0.2, 0.0), + Quaternion.from_euler_angles(0.6, 0.5, 0.4), + Quaternion.from_euler_angles(0.2, 0.8, 0.7), + Quaternion.from_euler_angles(0.0, 0.4, 1.0), +] + +# Create spline (auto-selects SQUAD for 4+ points, SLERP for 2) +quat_spline = QuaternionSpline(times, orientations, interpolation_method="auto") + +# Evaluate at any time +q, segment = quat_spline.interpolate_at_time(2.5) +print(f"Orientation at t=2.5: euler={q.to_euler_angles()}") + +# With angular velocity +q, angular_velocity, segment = quat_spline.interpolate_with_velocity(2.5) +``` + +## SquadC2: Cยฒ-Continuous Rotation Splines + +`SquadC2` is the most advanced quaternion interpolator. It provides: + +- **Cยฒ continuity**: Smooth angular velocity and acceleration +- **Zero-clamped boundaries**: Zero angular velocity/acceleration at endpoints +- **Non-uniform timing**: Supports variable segment durations + +```python +from interpolatepy import SquadC2, Quaternion +import numpy as np +import matplotlib.pyplot as plt + +# Define rotation keyframes +times = [0.0, 1.0, 2.0, 3.0, 4.0] +orientations = [ + Quaternion.identity(), + Quaternion.from_angle_axis(np.pi / 4, np.array([1, 0, 0])), + Quaternion.from_angle_axis(np.pi / 2, np.array([0, 1, 0])), + Quaternion.from_angle_axis(np.pi / 3, np.array([0, 0, 1])), + Quaternion.identity(), +] + +# Create Cยฒ continuous spline +squad = SquadC2(times, orientations) + +# Evaluate trajectory +t_eval = np.linspace(0, 4, 200) +euler_angles = [] +angular_velocities = [] + +for t in t_eval: + q = squad.evaluate(t) + omega = squad.evaluate_velocity(t) + euler_angles.append(q.to_euler_angles()) + angular_velocities.append(omega) + +euler_angles = np.array(euler_angles) +angular_velocities = np.array(angular_velocities) + +# Plot orientation and angular velocity +fig, axes = plt.subplots(2, 1, figsize=(12, 8)) + +for i, label in enumerate(["Roll", "Pitch", "Yaw"]): + axes[0].plot(t_eval, np.degrees(euler_angles[:, i]), label=label) +axes[0].set_ylabel("Angle (deg)") +axes[0].set_title("SquadC2: Orientation") +axes[0].legend() +axes[0].grid(True) + +for i, label in enumerate(["wx", "wy", "wz"]): + axes[1].plot(t_eval, angular_velocities[:, i], label=label) +axes[1].set_ylabel("Angular velocity (rad/s)") +axes[1].set_xlabel("Time (s)") +axes[1].set_title("SquadC2: Angular Velocity (smooth, zero at endpoints)") +axes[1].legend() +axes[1].grid(True) + +plt.tight_layout() +plt.show() +``` + +## Log-Quaternion Interpolation + +`LogQuaternionInterpolation` maps quaternions to 3D logarithmic space, applies cubic B-spline interpolation there, and maps back. This provides smooth, continuously differentiable rotational motion: + +```python +from interpolatepy import LogQuaternionInterpolation, Quaternion +import numpy as np + +times = [0.0, 1.0, 2.0, 3.0] +quaternions = [ + Quaternion.identity(), + Quaternion.from_angle_axis(np.pi / 4, np.array([1, 0, 0])), + Quaternion.from_angle_axis(np.pi / 2, np.array([0, 1, 0])), + Quaternion.from_angle_axis(np.pi / 3, np.array([0, 0, 1])), +] + +log_interp = LogQuaternionInterpolation(times, quaternions) + +# Evaluate +q = log_interp.evaluate(1.5) +omega = log_interp.evaluate_velocity(1.5) +alpha = log_interp.evaluate_acceleration(1.5) +``` + +The `ModifiedLogQuaternionInterpolation` variant decouples angle and axis interpolation, which can give better behavior near 180-degree rotations. + +## Choosing the Right Method + +| Method | Continuity | Best For | +|--------|-----------|----------| +| `Quaternion.slerp()` | Cโฐ | Two orientations, constant angular velocity | +| `QuaternionSpline` (SQUAD) | Cยน | Multiple waypoints, general use | +| `SquadC2` | Cยฒ | Robotics, smooth angular acceleration needed | +| `LogQuaternionInterpolation` | Cยฒ | Large rotations, logarithmic space benefits | + +## Common Pitfalls + +### Double Cover + +Quaternions `q` and `-q` represent the same rotation. InterpolatePy handles this automatically in `SquadC2`, but be aware when constructing quaternions manually: + +```python +# These represent the SAME rotation +q1 = Quaternion(1, 0, 0, 0) +q2 = Quaternion(-1, 0, 0, 0) +``` + +### Near-180-Degree Rotations + +Interpolation near 180-degree rotations can be problematic because the interpolation path becomes ambiguous. `SquadC2` handles this better than basic SLERP. + +## Next Steps + +- **[API Reference](../api-reference.md#quaternion)** for complete method documentation +- **[Algorithms Guide](../algorithms.md)** for mathematical foundations +- **Example scripts**: `examples/squad_c2_ex.py`, `examples/log_quat_ex.py`, `examples/quat_visualization_ex.py` diff --git a/docs/user-guide.md b/docs/user-guide.md index 9181878..bad447c 100644 --- a/docs/user-guide.md +++ b/docs/user-guide.md @@ -281,8 +281,8 @@ axes[0, 1].set_ylabel('Angular Speed (deg/s)') axes[0, 1].grid(True) # 3D trajectory visualization (quaternion components) -quaternion_components = np.array([[q.w, q.x, q.y, q.z] for q in orientations]) -for i, label in enumerate(['w', 'x', 'y', 'z']): +quaternion_components = np.array([[q.s_, *q.v_] for q in orientations]) +for i, label in enumerate(['s', 'v1', 'v2', 'v3']): axes[1, 0].plot(t_eval, quaternion_components[:, i], label=f'q_{label}', linewidth=2) axes[1, 0].set_title('Quaternion Components') diff --git a/examples/double_s_ex.py b/examples/double_s_ex.py index da302a4..71f5f67 100644 --- a/examples/double_s_ex.py +++ b/examples/double_s_ex.py @@ -47,7 +47,7 @@ def example_standard_trajectory() -> None: time_array = np.arange(0, t_duration + t_sample, t_sample) # Evaluate trajectory at those time points - positions, velocities, accelerations, jerks = trajectory.evaluate(time_array) + positions, velocities, accelerations, jerks = trajectory.evaluate_full(time_array) # Get phase durations phase_durations = trajectory.get_phase_durations() @@ -135,7 +135,7 @@ def example_velocity_matching() -> None: time_array = np.arange(0, t_duration + t_sample, t_sample) # Evaluate trajectory at those time points - positions, velocities, accelerations, jerks = trajectory.evaluate(time_array) + positions, velocities, accelerations, jerks = trajectory.evaluate_full(time_array) # Plot results fig, (ax1, ax2, ax3, ax4) = plt.subplots(4, 1, figsize=(12, 10), sharex=True) @@ -202,7 +202,7 @@ def example_negative_displacement() -> None: time_array = np.arange(0, t_duration + t_sample, t_sample) # Evaluate trajectory at those time points - positions, velocities, accelerations, jerks = trajectory.evaluate(time_array) + positions, velocities, accelerations, jerks = trajectory.evaluate_full(time_array) # Get phase durations phase_durations = trajectory.get_phase_durations() @@ -290,7 +290,7 @@ def example_asymmetric_velocities() -> None: time_array = np.arange(0, t_duration + t_sample, t_sample) # Evaluate trajectory at those time points - positions, velocities, accelerations, jerks = trajectory.evaluate(time_array) + positions, velocities, accelerations, jerks = trajectory.evaluate_full(time_array) # Get phase durations phase_durations = trajectory.get_phase_durations() diff --git a/examples/protocols_ex.py b/examples/protocols_ex.py new file mode 100644 index 0000000..c7dce9d --- /dev/null +++ b/examples/protocols_ex.py @@ -0,0 +1,476 @@ +""" +Example demonstrating protocol-based generic functions in InterpolatePy. + +Protocols (PEP 544 structural typing) allow writing algorithm-agnostic code. +A function annotated with a protocol type accepts ANY class that has matching +method signatures โ€” no inheritance required. + +This file showcases four protocol families: +1. ScalarTrajectory: CubicSpline vs DoubleSTrajectory +2. CurveEvaluator: BSplineInterpolator vs BSpline +3. GeometricPath: LinearPath vs CircularPath +4. QuaternionTrajectory: SquadC2 vs QuaternionSpline +""" + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D # noqa: F401 + +from interpolatepy import ( + # Protocols + CurveEvaluator, + GeometricPath, + QuaternionTrajectory, + ScalarTrajectory, + # Concrete classes + BSpline, + BSplineInterpolator, + CircularPath, + CubicSpline, + DoubleSTrajectory, + LinearPath, + Quaternion, + QuaternionSpline, + SquadC2, + StateParams, + TrajectoryBounds, +) + + +# --------------------------------------------------------------------------- +# 1. ScalarTrajectory โ€” generic sampling across spline & motion-profile +# --------------------------------------------------------------------------- + + +def sample_scalar_trajectory( + traj: ScalarTrajectory, + t_start: float, + t_end: float, + num_points: int = 200, +) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Sample position, velocity, and acceleration from any ScalarTrajectory. + + Parameters + ---------- + traj : ScalarTrajectory + Any object conforming to the ScalarTrajectory protocol. + t_start : float + Start time. + t_end : float + End time. + num_points : int + Number of sample points. + + Returns + ------- + tuple of np.ndarray + (time, position, velocity, acceleration) arrays. + """ + t = np.linspace(t_start, t_end, num_points) + pos = np.array([traj.evaluate(ti) for ti in t]) + vel = np.array([traj.evaluate_velocity(ti) for ti in t]) + acc = np.array([traj.evaluate_acceleration(ti) for ti in t]) + return t, pos, vel, acc + + +def example_scalar_trajectory() -> None: + """Compare CubicSpline and DoubleSTrajectory through the same generic function.""" + print("\n1. ScalarTrajectory Protocol") + print("=" * 40) + + # --- CubicSpline --- + t_points = [0.0, 1.0, 2.0, 3.0, 4.0] + q_points = [0.0, 5.0, 3.0, 8.0, 10.0] + spline = CubicSpline(t_points, q_points) + print(f" CubicSpline conforms: {isinstance(spline, ScalarTrajectory)}") + + # --- DoubleSTrajectory --- + state = StateParams(q_0=0.0, q_1=10.0, v_0=0.0, v_1=0.0) + bounds = TrajectoryBounds(v_bound=8.0, a_bound=20.0, j_bound=60.0) + double_s = DoubleSTrajectory(state, bounds) + duration = double_s.get_duration() + print(f" DoubleSTrajectory conforms: {isinstance(double_s, ScalarTrajectory)}") + + # Sample both through the *same* generic function + t_cs, pos_cs, vel_cs, acc_cs = sample_scalar_trajectory(spline, 0.0, 4.0) + t_ds, pos_ds, vel_ds, acc_ds = sample_scalar_trajectory(double_s, 0.0, duration) + + # Plot side-by-side + fig, axes = plt.subplots(3, 2, figsize=(14, 8), sharex="col") + fig.suptitle("ScalarTrajectory Protocol โ€” same function, two algorithms") + + labels = ["Position", "Velocity", "Acceleration"] + cs_data = [pos_cs, vel_cs, acc_cs] + ds_data = [pos_ds, vel_ds, acc_ds] + + for row, (label, cs_vals, ds_vals) in enumerate(zip(labels, cs_data, ds_data)): + axes[row, 0].plot(t_cs, cs_vals, "b-", linewidth=2) + axes[row, 0].set_ylabel(label) + axes[row, 0].grid(True) + if row == 0: + axes[row, 0].set_title("CubicSpline") + + axes[row, 1].plot(t_ds, ds_vals, "r-", linewidth=2) + axes[row, 1].grid(True) + if row == 0: + axes[row, 1].set_title("DoubleSTrajectory") + + axes[2, 0].set_xlabel("Time (s)") + axes[2, 1].set_xlabel("Time (s)") + plt.tight_layout() + plt.show() + + +# --------------------------------------------------------------------------- +# 2. CurveEvaluator โ€” unified derivative evaluation across B-spline variants +# --------------------------------------------------------------------------- + + +def evaluate_curve( + curve: CurveEvaluator, + u_start: float, + u_end: float, + num_points: int = 200, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Sample a parametric curve and its first derivative. + + Parameters + ---------- + curve : CurveEvaluator + Any object conforming to the CurveEvaluator protocol. + u_start : float + Start parameter value. + u_end : float + End parameter value. + num_points : int + Number of sample points. + + Returns + ------- + tuple of np.ndarray + (parameter, positions, derivatives) arrays. + """ + u = np.linspace(u_start, u_end, num_points) + positions = np.array([curve.evaluate(ui) for ui in u]) + derivatives = np.array([curve.evaluate_derivative(ui, order=1) for ui in u]) + return u, positions, derivatives + + +def example_curve_evaluator() -> None: + """Compare BSplineInterpolator and BSpline through the same generic function.""" + print("\n2. CurveEvaluator Protocol") + print("=" * 40) + + # --- BSplineInterpolator (interpolates through points) --- + points_2d = np.array([ + [0.0, 0.0], + [1.0, 2.0], + [3.0, 3.0], + [5.0, 1.0], + [6.0, 4.0], + ]) + interpolator = BSplineInterpolator(degree=3, points=points_2d) + print(f" BSplineInterpolator conforms: {isinstance(interpolator, CurveEvaluator)}") + + # --- BSpline (approximation with control polygon) --- + control_pts = np.array([ + [0.0, 0.0], + [1.0, 3.0], + [2.5, 4.0], + [4.0, 2.0], + [5.5, 3.5], + [6.0, 1.0], + ]) + n_ctrl = len(control_pts) + degree = 3 + n_knots = n_ctrl + degree + 1 + knots = np.concatenate([ + np.zeros(degree + 1), + np.linspace(0, 1, n_knots - 2 * (degree + 1) + 2)[1:-1], + np.ones(degree + 1), + ]) + bspline = BSpline(degree=degree, knots=knots, control_points=control_pts) + print(f" BSpline conforms: {isinstance(bspline, CurveEvaluator)}") + + # Sample both through the *same* generic function + _, pos_interp, der_interp = evaluate_curve(interpolator, interpolator.u_min, interpolator.u_max) + _, pos_bs, der_bs = evaluate_curve(bspline, bspline.u_min, bspline.u_max) + + # Plot + fig, axes = plt.subplots(1, 2, figsize=(14, 5)) + fig.suptitle("CurveEvaluator Protocol โ€” same function, two B-spline variants") + + # BSplineInterpolator + axes[0].plot(pos_interp[:, 0], pos_interp[:, 1], "b-", linewidth=2, label="Curve") + axes[0].plot(points_2d[:, 0], points_2d[:, 1], "ko", markersize=7, label="Data points") + # Tangent arrows at a few sample locations + step = len(pos_interp) // 8 + scale = 0.3 + for i in range(0, len(pos_interp), step): + axes[0].annotate( + "", + xy=(pos_interp[i, 0] + der_interp[i, 0] * scale, pos_interp[i, 1] + der_interp[i, 1] * scale), + xytext=(pos_interp[i, 0], pos_interp[i, 1]), + arrowprops={"arrowstyle": "->", "color": "green", "lw": 1.5}, + ) + axes[0].set_title("BSplineInterpolator") + axes[0].set_xlabel("X") + axes[0].set_ylabel("Y") + axes[0].legend() + axes[0].grid(True) + axes[0].set_aspect("equal", adjustable="datalim") + + # BSpline + axes[1].plot(pos_bs[:, 0], pos_bs[:, 1], "r-", linewidth=2, label="Curve") + axes[1].plot(control_pts[:, 0], control_pts[:, 1], "ks--", markersize=6, alpha=0.5, label="Control polygon") + step = len(pos_bs) // 8 + for i in range(0, len(pos_bs), step): + axes[1].annotate( + "", + xy=(pos_bs[i, 0] + der_bs[i, 0] * scale, pos_bs[i, 1] + der_bs[i, 1] * scale), + xytext=(pos_bs[i, 0], pos_bs[i, 1]), + arrowprops={"arrowstyle": "->", "color": "green", "lw": 1.5}, + ) + axes[1].set_title("BSpline") + axes[1].set_xlabel("X") + axes[1].set_ylabel("Y") + axes[1].legend() + axes[1].grid(True) + axes[1].set_aspect("equal", adjustable="datalim") + + plt.tight_layout() + plt.show() + + +# --------------------------------------------------------------------------- +# 3. GeometricPath โ€” path-agnostic 3D visualization with velocity arrows +# --------------------------------------------------------------------------- + + +def trace_geometric_path( + path: GeometricPath, + s_start: float, + s_end: float, + num_points: int = 200, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Trace position and velocity vectors along any GeometricPath. + + Parameters + ---------- + path : GeometricPath + Any object conforming to the GeometricPath protocol. + s_start : float + Start arc-length parameter. + s_end : float + End arc-length parameter. + num_points : int + Number of sample points. + + Returns + ------- + tuple of np.ndarray + (positions, velocities, accelerations) arrays of shape (N, 3). + """ + s_vals = np.linspace(s_start, s_end, num_points) + positions = np.array([path.position(si) for si in s_vals]) + velocities = np.array([path.velocity(si) for si in s_vals]) + accelerations = np.array([path.acceleration(si) for si in s_vals]) + return positions, velocities, accelerations + + +def example_geometric_path() -> None: + """Compare LinearPath and CircularPath through the same generic function.""" + print("\n3. GeometricPath Protocol") + print("=" * 40) + + # --- LinearPath --- + start = np.array([0.0, 0.0, 0.0]) + end = np.array([5.0, 3.0, 2.0]) + linear = LinearPath(start, end) + print(f" LinearPath conforms: {isinstance(linear, GeometricPath)}") + + # --- CircularPath --- + axis = np.array([0.0, 0.0, 1.0]) + center = np.array([0.0, 0.0, 0.0]) + point_on_circle = np.array([3.0, 0.0, 0.0]) + circular = CircularPath(axis, center, point_on_circle) + arc_length = np.pi * circular.radius # half circle + print(f" CircularPath conforms: {isinstance(circular, GeometricPath)}") + + # Sample both through the *same* generic function + pos_lin, vel_lin, _ = trace_geometric_path(linear, 0.0, linear.length) + pos_cir, vel_cir, acc_cir = trace_geometric_path(circular, 0.0, float(arc_length)) + + # 3D plot with velocity quiver arrows + fig = plt.figure(figsize=(14, 6)) + fig.suptitle("GeometricPath Protocol โ€” same function, two path types") + + # LinearPath + ax1 = fig.add_subplot(121, projection="3d") + ax1.plot(pos_lin[:, 0], pos_lin[:, 1], pos_lin[:, 2], "b-", linewidth=2, label="Path") + step = max(1, len(pos_lin) // 10) + ax1.quiver( + pos_lin[::step, 0], pos_lin[::step, 1], pos_lin[::step, 2], + vel_lin[::step, 0], vel_lin[::step, 1], vel_lin[::step, 2], + length=0.5, color="green", arrow_length_ratio=0.3, label="Velocity", + ) + ax1.set_title("LinearPath") + ax1.set_xlabel("X") + ax1.set_ylabel("Y") + ax1.set_zlabel("Z") + ax1.legend() + + # CircularPath + ax2 = fig.add_subplot(122, projection="3d") + ax2.plot(pos_cir[:, 0], pos_cir[:, 1], pos_cir[:, 2], "r-", linewidth=2, label="Path") + step = max(1, len(pos_cir) // 12) + ax2.quiver( + pos_cir[::step, 0], pos_cir[::step, 1], pos_cir[::step, 2], + vel_cir[::step, 0], vel_cir[::step, 1], vel_cir[::step, 2], + length=0.8, color="green", arrow_length_ratio=0.3, label="Velocity", + ) + ax2.quiver( + pos_cir[::step, 0], pos_cir[::step, 1], pos_cir[::step, 2], + acc_cir[::step, 0], acc_cir[::step, 1], acc_cir[::step, 2], + length=0.8, color="orange", arrow_length_ratio=0.3, label="Acceleration", + ) + ax2.set_title("CircularPath") + ax2.set_xlabel("X") + ax2.set_ylabel("Y") + ax2.set_zlabel("Z") + ax2.legend() + + plt.tight_layout() + plt.show() + + +# --------------------------------------------------------------------------- +# 4. QuaternionTrajectory โ€” rotation algorithm interchangeability +# --------------------------------------------------------------------------- + + +def sample_rotation( + traj: QuaternionTrajectory, + t_start: float, + t_end: float, + num_points: int = 200, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Extract Euler angles and angular velocity magnitude from any QuaternionTrajectory. + + Parameters + ---------- + traj : QuaternionTrajectory + Any object conforming to the QuaternionTrajectory protocol. + t_start : float + Start time. + t_end : float + End time. + num_points : int + Number of sample points. + + Returns + ------- + tuple of np.ndarray + (time, euler_angles (N,3), angular_velocity_magnitude (N,)) arrays. + """ + t = np.linspace(t_start, t_end, num_points) + euler = np.zeros((num_points, 3)) + omega_mag = np.zeros(num_points) + + for i, ti in enumerate(t): + q = traj.evaluate(ti) + euler[i] = q.to_euler_angles() + omega = traj.evaluate_velocity(ti) + omega_mag[i] = float(np.linalg.norm(omega)) + + return t, euler, omega_mag + + +def example_quaternion_trajectory() -> None: + """Compare SquadC2 and QuaternionSpline through the same generic function.""" + print("\n4. QuaternionTrajectory Protocol") + print("=" * 40) + + # Shared waypoints: identity โ†’ 90ยฐ about Z โ†’ 90ยฐ about X โ†’ back to identity + q1 = Quaternion.identity() + q2 = Quaternion.from_euler_angles(0.0, 0.0, np.pi / 2) + q3 = Quaternion.from_euler_angles(np.pi / 2, 0.0, np.pi / 2) + q4 = Quaternion.identity() + times = [0.0, 1.0, 2.0, 3.0] + quats = [q1, q2, q3, q4] + + # --- SquadC2 --- + squad = SquadC2(time_points=times, quaternions=quats) + print(f" SquadC2 conforms: {isinstance(squad, QuaternionTrajectory)}") + + # --- QuaternionSpline (SQUAD method) --- + qspline = QuaternionSpline(time_points=times, quaternions=quats, interpolation_method=Quaternion.SQUAD) + print(f" QuaternionSpline conforms: {isinstance(qspline, QuaternionTrajectory)}") + + # Sample both through the *same* generic function + t_sq, euler_sq, omega_sq = sample_rotation(squad, 0.0, 3.0) + t_qs, euler_qs, omega_qs = sample_rotation(qspline, 0.0, 3.0) + + # Plot + fig, axes = plt.subplots(2, 2, figsize=(14, 8), sharex="col") + fig.suptitle("QuaternionTrajectory Protocol โ€” same function, two rotation algorithms") + + angle_labels = ["Roll", "Pitch", "Yaw"] + colors = ["r", "g", "b"] + + # SquadC2 โ€” Euler angles + for j in range(3): + axes[0, 0].plot(t_sq, np.degrees(euler_sq[:, j]), color=colors[j], linewidth=2, label=angle_labels[j]) + axes[0, 0].set_ylabel("Angle (deg)") + axes[0, 0].set_title("SquadC2 โ€” Euler Angles") + axes[0, 0].legend() + axes[0, 0].grid(True) + + # SquadC2 โ€” Angular velocity magnitude + axes[1, 0].plot(t_sq, omega_sq, "m-", linewidth=2) + axes[1, 0].set_ylabel("||ฯ‰|| (rad/s)") + axes[1, 0].set_xlabel("Time (s)") + axes[1, 0].set_title("SquadC2 โ€” Angular Velocity") + axes[1, 0].grid(True) + + # QuaternionSpline โ€” Euler angles + for j in range(3): + axes[0, 1].plot(t_qs, np.degrees(euler_qs[:, j]), color=colors[j], linewidth=2, label=angle_labels[j]) + axes[0, 1].set_ylabel("Angle (deg)") + axes[0, 1].set_title("QuaternionSpline โ€” Euler Angles") + axes[0, 1].legend() + axes[0, 1].grid(True) + + # QuaternionSpline โ€” Angular velocity magnitude + axes[1, 1].plot(t_qs, omega_qs, "m-", linewidth=2) + axes[1, 1].set_ylabel("||ฯ‰|| (rad/s)") + axes[1, 1].set_xlabel("Time (s)") + axes[1, 1].set_title("QuaternionSpline โ€” Angular Velocity") + axes[1, 1].grid(True) + + plt.tight_layout() + plt.show() + + +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- + + +def main() -> None: + """Run all protocol demonstration examples.""" + print("Protocol-Based Generic Functions โ€” InterpolatePy") + print("=" * 50) + + try: + example_scalar_trajectory() + example_curve_evaluator() + example_geometric_path() + example_quaternion_trajectory() + print("\nAll protocol examples completed successfully!") + except (ValueError, TypeError, RuntimeError) as e: + print(f"Error running examples: {e}") + + +if __name__ == "__main__": + main() diff --git a/interpolatepy/__init__.py b/interpolatepy/__init__.py index 9c1879a..bdd221d 100644 --- a/interpolatepy/__init__.py +++ b/interpolatepy/__init__.py @@ -3,63 +3,68 @@ This package provides smooth trajectory generation with precise control over position, velocity, acceleration, and jerk profiles for robotics, animation, and scientific computing. + +When the compiled C++ extension is available, computation-heavy classes are +automatically backed by the native implementation. Set the environment variable +``INTERPOLATEPY_NO_CPP=1`` to force pure-Python mode. """ from .version import __version__ -# Core spline algorithms -from .cubic_spline import CubicSpline -from .c_s_smoothing import CubicSmoothingSpline -from .c_s_smoot_search import SplineConfig -from .c_s_smoot_search import smoothing_spline_with_tolerance -from .c_s_with_acc1 import CubicSplineWithAcceleration1 -from .c_s_with_acc2 import CubicSplineWithAcceleration2 -from .c_s_with_acc2 import SplineParameters +from ._backend import HAS_CPP -# B-spline family -from .b_spline import BSpline -from .b_spline_approx import ApproximationBSpline -from .b_spline_cubic import CubicBSplineInterpolation -from .b_spline_interpolate import BSplineInterpolator -from .b_spline_smooth import BSplineParams -from .b_spline_smooth import SmoothingCubicBSpline +# โ”€โ”€ Backend-aware classes and functions โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._api import ( + ApproximationBSpline, + BSpline, + BSplineInterpolator, + BSplineParams, + BoundaryCondition, + CircularPath, + CubicBSplineInterpolation, + CubicSmoothingSpline, + CubicSpline, + CubicSplineWithAcceleration1, + CubicSplineWithAcceleration2, + DoubleSTrajectory, + LinearPath, + LogQuaternionInterpolation, + ModifiedLogQuaternionInterpolation, + ParabolicBlendTrajectory, + PolynomialTrajectory, + QuaternionSpline, + SmoothingCubicBSpline, + SplineConfig, + SplineParameters, + SquadC2, + StateParams, + TimeInterval, + TrajectoryBounds, + TrapezoidalTrajectory, + circular_trajectory_with_derivatives, + compute_trajectory_frames, + helicoidal_trajectory_with_derivatives, + linear_traj, + smoothing_spline_with_tolerance, + solve_tridiagonal, +) -# Motion profiles -from .double_s import DoubleSTrajectory -from .double_s import StateParams -from .double_s import TrajectoryBounds -from .polynomials import BoundaryCondition -from .polynomials import PolynomialTrajectory -from .polynomials import TimeInterval +# โ”€โ”€ Always pure-Python (no C++ equivalent) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ from .polynomials import TrajectoryParams from .trapezoidal import CalculationParams from .trapezoidal import InterpolationParams -from .trapezoidal import TrapezoidalTrajectory - -# Path planning -from .simple_paths import CircularPath -from .simple_paths import LinearPath -from .lin_poly_parabolic import ParabolicBlendTrajectory - -# Quaternion interpolation from .quat_core import Quaternion -from .quat_spline import QuaternionSpline -from .squad_c2 import SquadC2 - -# Linear interpolation -from .linear import linear_traj - -# Frenet frame utilities -from .frenet_frame import compute_trajectory_frames -from .frenet_frame import circular_trajectory_with_derivatives -from .frenet_frame import helicoidal_trajectory_with_derivatives from .frenet_frame import plot_frames -# Utility functions -from .tridiagonal_inv import solve_tridiagonal +# Protocols +from .protocols import CurveEvaluator +from .protocols import GeometricPath +from .protocols import QuaternionTrajectory +from .protocols import ScalarTrajectory +from .protocols import TrajectoryFunction __all__ = [ - # Core spline algorithms + "HAS_CPP", "ApproximationBSpline", "BSpline", "BSplineInterpolator", @@ -72,13 +77,19 @@ "CubicSpline", "CubicSplineWithAcceleration1", "CubicSplineWithAcceleration2", + "CurveEvaluator", "DoubleSTrajectory", + "GeometricPath", "InterpolationParams", "LinearPath", + "LogQuaternionInterpolation", + "ModifiedLogQuaternionInterpolation", "ParabolicBlendTrajectory", "PolynomialTrajectory", "Quaternion", "QuaternionSpline", + "QuaternionTrajectory", + "ScalarTrajectory", "SmoothingCubicBSpline", "SplineConfig", "SplineParameters", @@ -86,9 +97,9 @@ "StateParams", "TimeInterval", "TrajectoryBounds", + "TrajectoryFunction", "TrajectoryParams", "TrapezoidalTrajectory", - # Version and functions "__version__", "circular_trajectory_with_derivatives", "compute_trajectory_frames", diff --git a/interpolatepy/_adapters/__init__.py b/interpolatepy/_adapters/__init__.py new file mode 100644 index 0000000..dabd5b6 --- /dev/null +++ b/interpolatepy/_adapters/__init__.py @@ -0,0 +1,100 @@ +"""C++ backed classes with Python-only convenience methods. + +This package is only imported when ``_backend.HAS_CPP`` is ``True``. Each +sub-module subclasses the pybind11-exposed C++ class and bolts on any +Python-only methods (``plot()``, ``__repr__``, etc.) so the public API stays +identical regardless of the active backend. +""" + +from __future__ import annotations + +# โ”€โ”€ Spline family โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._spline import ( + CubicSmoothingSpline, + CubicSpline, + CubicSplineWithAcceleration1, + CubicSplineWithAcceleration2, +) + +# โ”€โ”€ B-spline family โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._bspline import ( + ApproximationBSpline, + BSpline, + BSplineInterpolator, + CubicBSplineInterpolation, + SmoothingCubicBSpline, +) + +# โ”€โ”€ Motion profiles โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._motion import ( + DoubleSTrajectory, + ParabolicBlendTrajectory, + PolynomialTrajectory, + TrapezoidalTrajectory, +) + +# โ”€โ”€ Quaternion interpolation โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._quaternion import ( + LogQuaternionInterpolation, + ModifiedLogQuaternionInterpolation, + QuaternionSpline, + SquadC2, +) + +# โ”€โ”€ Path adapters โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._paths import ( + CircularPath, + LinearPath, +) + +# โ”€โ”€ Direct re-exports (no adapter needed) โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ +from ._direct import ( + BoundaryCondition, + BSplineParams, + circular_trajectory_with_derivatives, + compute_trajectory_frames, + helicoidal_trajectory_with_derivatives, + linear_traj, + smoothing_spline_with_tolerance, + solve_tridiagonal, + SplineConfig, + SplineParameters, + StateParams, + TimeInterval, + TrajectoryBounds, +) + +__all__ = [ + "ApproximationBSpline", + "BSpline", + "BSplineInterpolator", + "BSplineParams", + "BoundaryCondition", + "CircularPath", + "CubicBSplineInterpolation", + "CubicSmoothingSpline", + "CubicSpline", + "CubicSplineWithAcceleration1", + "CubicSplineWithAcceleration2", + "DoubleSTrajectory", + "LinearPath", + "LogQuaternionInterpolation", + "ModifiedLogQuaternionInterpolation", + "ParabolicBlendTrajectory", + "PolynomialTrajectory", + "QuaternionSpline", + "SmoothingCubicBSpline", + "SplineConfig", + "SplineParameters", + "SquadC2", + "StateParams", + "TimeInterval", + "TrajectoryBounds", + "TrapezoidalTrajectory", + "circular_trajectory_with_derivatives", + "compute_trajectory_frames", + "helicoidal_trajectory_with_derivatives", + "linear_traj", + "smoothing_spline_with_tolerance", + "solve_tridiagonal", +] diff --git a/interpolatepy/_adapters/_bspline.py b/interpolatepy/_adapters/_bspline.py new file mode 100644 index 0000000..31b9f37 --- /dev/null +++ b/interpolatepy/_adapters/_bspline.py @@ -0,0 +1,77 @@ +"""Adapters for the B-spline family. + +Adds ``plot_2d()``, ``plot_3d()``, ``__repr__`` and plotting helpers that +exist only in the pure-Python implementations. +""" + +from __future__ import annotations + +from interpolatepy._backend import get_cpp_module +from interpolatepy.b_spline import BSpline as _PyBSpline +from interpolatepy.b_spline_interpolate import BSplineInterpolator as _PyBSplineInterpolator + +_cpp = get_cpp_module() + +_CppBSpline = _cpp.bspline.BSpline +_CppCubicBSplineInterpolation = _cpp.bspline.CubicBSplineInterpolation +_CppBSplineInterpolator = _cpp.bspline.BSplineInterpolator +_CppApproximationBSpline = _cpp.bspline.ApproximationBSpline +_CppSmoothingCubicBSpline = _cpp.bspline.SmoothingCubicBSpline + + +class BSpline(_CppBSpline): # type: ignore[valid-type, misc] + """C++-backed BSpline with Python plotting and repr.""" + + # Class constants used by plot methods + DIM_2 = 2 + DIM_3 = 3 + + plot_2d = _PyBSpline.plot_2d + plot_3d = _PyBSpline.plot_3d + __repr__ = _PyBSpline.__repr__ + + +class CubicBSplineInterpolation(_CppCubicBSplineInterpolation): # type: ignore[valid-type, misc] + """C++-backed CubicBSplineInterpolation.""" + + DIM_2 = 2 + DIM_3 = 3 + + plot_2d = _PyBSpline.plot_2d + plot_3d = _PyBSpline.plot_3d + __repr__ = _PyBSpline.__repr__ + + +class BSplineInterpolator(_CppBSplineInterpolator): # type: ignore[valid-type, misc] + """C++-backed BSplineInterpolator with plotting helpers.""" + + DIM_2 = 2 + DIM_3 = 3 + + plot_2d = _PyBSpline.plot_2d + plot_3d = _PyBSpline.plot_3d + plot_with_points = _PyBSplineInterpolator.plot_with_points + plot_with_points_3d = _PyBSplineInterpolator.plot_with_points_3d + __repr__ = _PyBSpline.__repr__ + + +class ApproximationBSpline(_CppApproximationBSpline): # type: ignore[valid-type, misc] + """C++-backed ApproximationBSpline.""" + + DIM_2 = 2 + DIM_3 = 3 + + plot_2d = _PyBSpline.plot_2d + plot_3d = _PyBSpline.plot_3d + __repr__ = _PyBSpline.__repr__ + + +class SmoothingCubicBSpline(_CppSmoothingCubicBSpline): # type: ignore[valid-type, misc] + """C++-backed SmoothingCubicBSpline.""" + + DIM_2 = 2 + DIM_3 = 3 + + plot_2d = _PyBSpline.plot_2d + plot_3d = _PyBSpline.plot_3d + __repr__ = _PyBSpline.__repr__ diff --git a/interpolatepy/_adapters/_direct.py b/interpolatepy/_adapters/_direct.py new file mode 100644 index 0000000..e40c570 --- /dev/null +++ b/interpolatepy/_adapters/_direct.py @@ -0,0 +1,23 @@ +"""Direct re-exports from the C++ backend (no adapter wrapper needed).""" + +from interpolatepy._backend import get_cpp_module + +_cpp = get_cpp_module() + +# Data / config classes +SplineParameters = _cpp.SplineParameters +SplineConfig = _cpp.SplineConfig +BSplineParams = _cpp.bspline.BSplineParams + +StateParams = _cpp.motion.StateParams +TrajectoryBounds = _cpp.motion.TrajectoryBounds +BoundaryCondition = _cpp.motion.BoundaryCondition +TimeInterval = _cpp.motion.TimeInterval + +# Free functions +solve_tridiagonal = _cpp.solve_tridiagonal +smoothing_spline_with_tolerance = _cpp.smoothing_spline_with_tolerance +linear_traj = _cpp.path.linear_traj +compute_trajectory_frames = _cpp.path.compute_frenet_frames +circular_trajectory_with_derivatives = _cpp.path.circular_trajectory_with_derivatives +helicoidal_trajectory_with_derivatives = _cpp.path.helicoidal_trajectory_with_derivatives diff --git a/interpolatepy/_adapters/_motion.py b/interpolatepy/_adapters/_motion.py new file mode 100644 index 0000000..0e8e842 --- /dev/null +++ b/interpolatepy/_adapters/_motion.py @@ -0,0 +1,461 @@ +"""Adapters for motion profile classes. # noqa: PLC0415 + +``DoubleSTrajectory`` and ``ParabolicBlendTrajectory`` need adapters because +the C++ ``evaluate()`` returns a struct while the Python API exposes separate +``evaluate()``, ``evaluate_velocity()``, ``evaluate_acceleration()`` methods. + +``TrapezoidalTrajectory`` and ``PolynomialTrajectory`` need adapters because +the Python API uses class methods returning callables, while the C++ API is +instance-based with ``evaluate()`` methods. + +Deferred imports from ``interpolatepy.trapezoidal`` and ``interpolatepy.polynomials`` +are intentional to avoid circular imports (``__init__.py`` โ†’ ``_adapters`` โ†’ module). +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, ClassVar + +import numpy as np + +if TYPE_CHECKING: + from collections.abc import Callable + + from interpolatepy.polynomials import BoundaryCondition as PyBoundaryCondition + from interpolatepy.polynomials import TimeInterval as PyTimeInterval + from interpolatepy.polynomials import TrajectoryParams as PolyTrajectoryParams + from interpolatepy.trapezoidal import InterpolationParams + from interpolatepy.trapezoidal import TrajectoryParams as TrapTrajectoryParams + +from interpolatepy._backend import get_cpp_module + +_cpp = get_cpp_module() + +_CppDoubleSTrajectory = _cpp.motion.DoubleSTrajectory +_CppParabolicBlendTrajectory = _cpp.motion.ParabolicBlendTrajectory +_CppTrapezoidalTrajectory = _cpp.motion.TrapezoidalTrajectory +_CppPolynomialTrajectory = _cpp.motion.PolynomialTrajectory +_CppBoundaryCondition = _cpp.motion.BoundaryCondition +_CppTimeInterval = _cpp.motion.TimeInterval + + +class DoubleSTrajectory(_CppDoubleSTrajectory): # type: ignore[valid-type, misc] + """C++-backed DoubleSTrajectory matching the Python ScalarTrajectory protocol. + + The C++ class has a single ``evaluate(t) -> TrajectoryResult`` whereas the + Python protocol requires separate position / velocity / acceleration + methods. This adapter splits the result accordingly. + """ + + def evaluate(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).position + return out + return super().evaluate(t).position + + def evaluate_velocity(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).velocity + return out + return super().evaluate(t).velocity + + def evaluate_acceleration(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).acceleration + return out + return super().evaluate(t).acceleration + + def evaluate_jerk(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).jerk + return out + return super().evaluate(t).jerk + + def evaluate_full( + self, t: float | np.ndarray + ) -> tuple[float | np.ndarray, ...]: + """Evaluate position, velocity, acceleration, and jerk at time *t*.""" + if isinstance(t, np.ndarray): + pos = np.empty_like(t) + vel = np.empty_like(t) + acc = np.empty_like(t) + jrk = np.empty_like(t) + for i, ti in enumerate(t.flat): + r = super().evaluate(float(ti)) + pos.flat[i] = r.position + vel.flat[i] = r.velocity + acc.flat[i] = r.acceleration + jrk.flat[i] = r.jerk + return pos, vel, acc, jrk + r = super().evaluate(t) + return r.position, r.velocity, r.acceleration, r.jerk + + @property + def T(self) -> float: # noqa: N802 + """Total trajectory duration.""" + return self.duration + + +class ParabolicBlendTrajectory(_CppParabolicBlendTrajectory): # type: ignore[valid-type, misc] + """C++-backed ParabolicBlendTrajectory with evaluate protocol split.""" + + def evaluate(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).position + return out + return super().evaluate(t).position + + def evaluate_velocity(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).velocity + return out + return super().evaluate(t).velocity + + def evaluate_acceleration(self, t: float | np.ndarray) -> float | np.ndarray: + if isinstance(t, np.ndarray): + out = np.empty_like(t) + for i, ti in enumerate(t.flat): + out.flat[i] = super().evaluate(float(ti)).acceleration + return out + return super().evaluate(t).acceleration + + +# --------------------------------------------------------------------------- +# TrapezoidalTrajectory adapter +# --------------------------------------------------------------------------- + + +def _to_cpp_bc(bc: PyBoundaryCondition) -> object: + """Convert a Python BoundaryCondition to C++ BoundaryCondition.""" + cpp_bc = _CppBoundaryCondition() + cpp_bc.position = bc.position + cpp_bc.velocity = bc.velocity + cpp_bc.acceleration = bc.acceleration + cpp_bc.jerk = bc.jerk + return cpp_bc + + +def _to_cpp_ti(ti: PyTimeInterval) -> object: + """Convert a Python TimeInterval to C++ TimeInterval.""" + cpp_ti = _CppTimeInterval() + cpp_ti.start = ti.start + cpp_ti.end = ti.end + return cpp_ti + + +class TrapezoidalTrajectory: + """C++-backed TrapezoidalTrajectory matching the pure-Python class API. + + The pure-Python version exposes ``generate_trajectory()`` and + ``interpolate_waypoints()`` class methods that return *callables*. + The C++ class is instance-based with ``evaluate(t) -> TrajectoryResult``. + This adapter bridges the two by wrapping C++ instances inside closures. + """ + + @staticmethod + def generate_trajectory( + params: TrapTrajectoryParams, + ) -> tuple[Callable[[float], tuple[float, float, float]], float]: + """Generate a single-segment trapezoidal trajectory. + + Parameters + ---------- + params : TrajectoryParams + Parameters including q0, q1, v0, v1, amax, vmax, duration. + + Returns + ------- + tuple[Callable, float] + Trajectory callable and total duration. + """ + if params.amax is None: + raise ValueError("Maximum acceleration (amax) must be provided") + if params.duration is None and params.vmax is None: + raise ValueError( + "Either duration or maximum velocity (vmax) must be provided" + ) + + amax = abs(params.amax) + + if params.vmax is not None and params.duration is None: + # Velocity-based: delegate to C++ + vmax = abs(params.vmax) + cpp_traj = _CppTrapezoidalTrajectory( + params.q0, params.q1, amax, vmax, + params.v0, params.v1, params.t0, + ) + elif params.duration is not None: + # Duration-based: use C++ duration-based constructor (kwargs + # needed to disambiguate from velocity-based overload) + cpp_traj = _CppTrapezoidalTrajectory( + q0=params.q0, q1=params.q1, amax=amax, + v0=params.v0, v1=params.v1, t0=params.t0, + duration=params.duration, + ) + else: + raise ValueError( + "Invalid parameter combination. Provide either " + "(amax, vmax) or (amax, duration)." + ) + + duration = cpp_traj.duration + + def trajectory(t: float) -> tuple[float, float, float]: + r = cpp_traj.evaluate(t) + return r.position, r.velocity, r.acceleration + + return trajectory, duration + + @staticmethod + def calculate_heuristic_velocities( + q_list: list[float], + v0: float, + vn: float, + v_max: float | None = None, + amax: float | None = None, + ) -> list[float]: + """Calculate intermediate velocities heuristically.""" + from interpolatepy.trapezoidal import ( # noqa: PLC0415 + TrapezoidalTrajectory as _PyTrap, + ) + + return _PyTrap.calculate_heuristic_velocities( + q_list, v0, vn, v_max, amax + ) + + @classmethod + def interpolate_waypoints( + cls, + params: InterpolationParams, + ) -> tuple[Callable[[float], tuple[float, float, float]], float]: + """Generate a multi-segment trajectory through waypoints. + + Parameters + ---------- + params : InterpolationParams + Parameters including points, velocities, times, amax, vmax. + + Returns + ------- + tuple[Callable, float] + Trajectory callable and total duration. + """ + from interpolatepy.trapezoidal import ( # noqa: PLC0415 + TrapezoidalTrajectory as _PyTrap, + ) + + return _PyTrap.interpolate_waypoints(params) + + +# --------------------------------------------------------------------------- +# PolynomialTrajectory adapter +# --------------------------------------------------------------------------- + +_ORDER_3 = 3 +_ORDER_5 = 5 +_ORDER_7 = 7 + + +class PolynomialTrajectory: + """C++-backed PolynomialTrajectory matching the pure-Python class API. + + The pure-Python version exposes ``order_3_trajectory()`` etc. class methods + that return callables. The C++ class takes ``order`` in its constructor + and returns ``FullTrajectoryResult`` structs. This adapter creates C++ + instances and wraps their ``evaluate()`` in closures. + """ + + VALID_ORDERS: ClassVar[tuple[int, ...]] = (3, 5, 7) + + @staticmethod + def _make_callable( + initial: PyBoundaryCondition, + final: PyBoundaryCondition, + time: PyTimeInterval, + order: int, + ) -> Callable[[float], tuple[float, float, float, float]]: + """Create a C++ PolynomialTrajectory and wrap it in a callable.""" + cpp_traj = _CppPolynomialTrajectory( + _to_cpp_bc(initial), _to_cpp_bc(final), _to_cpp_ti(time), order + ) + + def trajectory(t: float) -> tuple[float, float, float, float]: + r = cpp_traj.evaluate(t) + return r.position, r.velocity, r.acceleration, r.jerk + + return trajectory + + @classmethod + def order_3_trajectory( + cls, + initial: PyBoundaryCondition, + final: PyBoundaryCondition, + time: PyTimeInterval, + ) -> Callable[[float], tuple[float, float, float, float]]: + """Generate a 3rd order polynomial trajectory.""" + return cls._make_callable(initial, final, time, 3) + + @classmethod + def order_5_trajectory( + cls, + initial: PyBoundaryCondition, + final: PyBoundaryCondition, + time: PyTimeInterval, + ) -> Callable[[float], tuple[float, float, float, float]]: + """Generate a 5th order polynomial trajectory.""" + return cls._make_callable(initial, final, time, 5) + + @classmethod + def order_7_trajectory( + cls, + initial: PyBoundaryCondition, + final: PyBoundaryCondition, + time: PyTimeInterval, + ) -> Callable[[float], tuple[float, float, float, float]]: + """Generate a 7th order polynomial trajectory.""" + return cls._make_callable(initial, final, time, 7) + + @staticmethod + def heuristic_velocities( + points: list[float], times: list[float] + ) -> list[float]: + """Compute heuristic intermediate velocities via C++.""" + return list( + _CppPolynomialTrajectory.heuristic_velocities(points, times) + ) + + @classmethod + def multipoint_trajectory( + cls, + params: PolyTrajectoryParams, + ) -> Callable[[float], tuple[float, float, float, float]]: + """Generate a multi-segment polynomial trajectory. + + Parameters + ---------- + params : TrajectoryParams + Parameters including points, times, order, velocities, etc. + + Returns + ------- + Callable + Function returning (position, velocity, acceleration, jerk). + """ + n = len(params.points) + + if n != len(params.times): + raise ValueError("Number of points and times must be the same") + + order = params.order + if order not in cls.VALID_ORDERS: + valid = ", ".join(str(o) for o in cls.VALID_ORDERS) + raise ValueError(f"Order must be one of: {valid}") + + # Use C++ multipoint when only basic params are needed + vel = params.velocities + acc = params.accelerations + jrk = params.jerks + + # For order 3 without custom vel/acc/jerk, use C++ static method + if order == _ORDER_3 and vel is None and acc is None and jrk is None: + cpp_segments = _CppPolynomialTrajectory.multipoint_trajectory( + params.points, params.times, order, 0.0, 0.0 + ) + + def _basic_trajectory(t: float) -> tuple[float, float, float, float]: + r = _CppPolynomialTrajectory.evaluate_multipoint( + cpp_segments, t + ) + return r.position, r.velocity, r.acceleration, r.jerk + + return _basic_trajectory + + # For higher orders or custom boundary conditions, build segments + if vel is None: + vel = cls.heuristic_velocities(params.points, params.times) + if acc is None and order in {_ORDER_5, _ORDER_7}: + acc = [0.0] * n + if jrk is None and order == _ORDER_7: + jrk = [0.0] * n + + from interpolatepy.polynomials import BoundaryCondition, TimeInterval # noqa: PLC0415 + + segments: list[ + tuple[ + Callable[[float], tuple[float, float, float, float]], + float, + float, + ] + ] = [] + + for i in range(n - 1): + time_interval = TimeInterval( + params.times[i], params.times[i + 1] + ) + if order == _ORDER_3: + bc_i = BoundaryCondition(params.points[i], vel[i]) + bc_f = BoundaryCondition( + params.points[i + 1], vel[i + 1] + ) + elif order == _ORDER_5: + assert acc is not None + bc_i = BoundaryCondition( + params.points[i], vel[i], acc[i] + ) + bc_f = BoundaryCondition( + params.points[i + 1], + vel[i + 1], + acc[i + 1], + ) + else: # order == 7 + assert acc is not None + assert jrk is not None + bc_i = BoundaryCondition( + params.points[i], + vel[i], + acc[i], + jrk[i], + ) + bc_f = BoundaryCondition( + params.points[i + 1], + vel[i + 1], + acc[i + 1], + jrk[i + 1], + ) + + seg_fn = cls._make_callable(bc_i, bc_f, time_interval, order) + segments.append( + (seg_fn, params.times[i], params.times[i + 1]) + ) + + times_list = params.times + + def trajectory(t: float) -> tuple[float, float, float, float]: + if t < times_list[0]: + return segments[0][0](times_list[0]) + if t > times_list[-1]: + return segments[-1][0](times_list[-1]) + left, right = 0, len(segments) - 1 + while left <= right: + mid = (left + right) // 2 + if segments[mid][1] <= t <= segments[mid][2]: + return segments[mid][0](t) + if t < segments[mid][1]: + right = mid - 1 + else: + left = mid + 1 + raise ValueError(f"No segment found for time {t}") + + return trajectory diff --git a/interpolatepy/_adapters/_paths.py b/interpolatepy/_adapters/_paths.py new file mode 100644 index 0000000..ca5f12f --- /dev/null +++ b/interpolatepy/_adapters/_paths.py @@ -0,0 +1,130 @@ +"""Adapters for geometric path classes. + +The C++ constructors use different parameter names from Python: +- ``LinearPath``: C++ ``pi, pf`` vs Python ``pi, pf`` (same) +- ``CircularPath``: C++ ``axis, axis_point, circle_point`` vs Python ``r, d, pi`` + +Adds ``evaluate_at()`` and ``all_traj()`` convenience methods that the pure-Python +classes provide but the C++ classes do not. +""" + +from __future__ import annotations + +import numpy as np + +from interpolatepy._backend import get_cpp_module + +_cpp = get_cpp_module() + +_CppLinearPath = _cpp.path.LinearPath +_CppCircularPath = _cpp.path.CircularPath + + +class LinearPath(_CppLinearPath): # type: ignore[valid-type, misc] + """C++-backed LinearPath with Python-compatible constructor.""" + + def __init__(self, pi: np.ndarray, pf: np.ndarray) -> None: + self._pi = np.asarray(pi, dtype=float) + self._pf = np.asarray(pf, dtype=float) + super().__init__(self._pi, self._pf) + diff = self._pf - self._pi + length = float(np.linalg.norm(diff)) + self._tangent = diff / length if length > 0 else np.zeros(3) + + @property + def pi(self) -> np.ndarray: + """Initial point.""" + return self._pi + + @property + def pf(self) -> np.ndarray: + """Final point.""" + return self._pf + + @property + def tangent(self) -> np.ndarray: + """Unit tangent vector.""" + return self._tangent + + @property + def total_length(self) -> float: + """Alias for ``length`` property.""" + return self.length + + def evaluate_at( + self, s_values: float | list[float] | np.ndarray + ) -> dict[str, np.ndarray]: + """Evaluate position, velocity, acceleration at arc-length values.""" + s_arr = np.atleast_1d(np.asarray(s_values, dtype=float)) + s_clipped = np.clip(s_arr, 0, self.length) + n = len(s_clipped) + positions = np.zeros((n, 3)) + for i, s in enumerate(s_clipped): + positions[i] = self.position(float(s)) + velocities = np.tile(self.velocity(0.0), (n, 1)) + accelerations = np.zeros((n, 3)) + return { + "position": positions, + "velocity": velocities, + "acceleration": accelerations, + "s": s_clipped, + } + + def all_traj(self, num_points: int = 100) -> dict[str, np.ndarray]: + """Generate complete trajectory along the path.""" + s_values = np.linspace(0, self.length, num_points) + return self.evaluate_at(s_values) + + +class CircularPath(_CppCircularPath): # type: ignore[valid-type, misc] + """C++-backed CircularPath with Python-compatible parameter names.""" + + def __init__(self, r: np.ndarray, d: np.ndarray, pi: np.ndarray) -> None: + self._r = np.asarray(r, dtype=float) + self._d = np.asarray(d, dtype=float) + self._pi = np.asarray(pi, dtype=float) + super().__init__( + axis=self._r, + axis_point=self._d, + circle_point=self._pi, + ) + + @property + def r(self) -> np.ndarray: + """Axis vector.""" + return self._r + + @property + def d(self) -> np.ndarray: + """Point on the axis.""" + return self._d + + @property + def pi(self) -> np.ndarray: + """Point on the circle.""" + return self._pi + + def evaluate_at( + self, s_values: float | list[float] | np.ndarray + ) -> dict[str, np.ndarray]: + """Evaluate position, velocity, acceleration at arc-length values.""" + s_arr = np.atleast_1d(np.asarray(s_values, dtype=float)) + n = len(s_arr) + positions = np.zeros((n, 3)) + velocities = np.zeros((n, 3)) + accelerations = np.zeros((n, 3)) + for i, s in enumerate(s_arr): + positions[i] = self.position(float(s)) + velocities[i] = self.velocity(float(s)) + accelerations[i] = self.acceleration(float(s)) + return { + "position": positions, + "velocity": velocities, + "acceleration": accelerations, + "s": s_arr, + } + + def all_traj(self, num_points: int = 100) -> dict[str, np.ndarray]: + """Generate complete trajectory around the circle.""" + s_values = np.linspace(0, 2 * np.pi * self.radius, num_points) + return self.evaluate_at(s_values) diff --git a/interpolatepy/_adapters/_quaternion.py b/interpolatepy/_adapters/_quaternion.py new file mode 100644 index 0000000..dabb1df --- /dev/null +++ b/interpolatepy/_adapters/_quaternion.py @@ -0,0 +1,177 @@ +"""Adapters for the quaternion interpolation family. + +The C++ ``evaluate()`` methods return C++ ``Quaternion`` objects, but user code +expects the Python ``Quaternion`` with its full API (dynamics methods, matrix +conversions, etc.). These adapters convert on the boundary: + +- Constructor: convert incoming Python Quaternions โ†’ C++ Quaternions +- ``evaluate()``: convert returned C++ Quaternions โ†’ Python Quaternions +""" + +from __future__ import annotations + +from typing import Any + +from interpolatepy._backend import get_cpp_module +from interpolatepy.quat_core import Quaternion as _PyQuaternion + +_cpp = get_cpp_module() + +_CppQuaternion = _cpp.quat.Quaternion +_CppQuaternionSpline = _cpp.quat.QuaternionSpline +_CppSquadC2 = _cpp.quat.SquadC2 +_CppLogQuaternionInterpolation = _cpp.quat.LogQuaternionInterpolation +_CppModifiedLogQuaternionInterpolation = _cpp.quat.ModifiedLogQuaternionInterpolation + + +def _py_to_cpp(q: _PyQuaternion) -> Any: + """Convert a Python Quaternion to a C++ Quaternion.""" + return _CppQuaternion(q.w, q.x, q.y, q.z) + + +def _cpp_to_py(q: Any) -> _PyQuaternion: + """Convert a C++ Quaternion to a Python Quaternion.""" + return _PyQuaternion(q.w, q.x, q.y, q.z) + + +class QuaternionSpline(_CppQuaternionSpline): # type: ignore[valid-type, misc] + """C++-backed QuaternionSpline returning Python Quaternions.""" + + def __init__( + self, + time_points: list[float], + quaternions: list[_PyQuaternion], + interpolation_method: str = "auto", + ) -> None: + cpp_quats = [_py_to_cpp(q) for q in quaternions] + # Map string method names to C++ enum + method_map = { + "slerp": _cpp.quat.QuaternionSplineMethod.Slerp, + "squad": _cpp.quat.QuaternionSplineMethod.Squad, + "auto": _cpp.quat.QuaternionSplineMethod.Auto, + } + cpp_method = method_map.get( + interpolation_method, + _cpp.quat.QuaternionSplineMethod.Auto, + ) + super().__init__(time_points, cpp_quats, cpp_method) + self._py_quaternions = list(quaternions) + self._method_str = interpolation_method + + def evaluate(self, t: float) -> _PyQuaternion: + if not self._py_quaternions: + msg = "Cannot evaluate an empty QuaternionSpline" + raise ValueError(msg) + return _cpp_to_py(super().evaluate(t)) + + @property + def interpolation_method(self) -> str: + """Return the method string used for construction.""" + return getattr(self, "_method_str", "auto") + + @interpolation_method.setter + def interpolation_method(self, value: str) -> None: + self._method_str = value + + @property + def quat_data(self) -> list[_PyQuaternion]: + """Original quaternion waypoints.""" + return getattr(self, "_py_quaternions", []) + + @quat_data.setter + def quat_data(self, value: object) -> None: + self._py_quaternions = value # type: ignore[assignment] + + def get_time_range(self) -> tuple[float, float]: + """Return (t_min, t_max).""" + return (self.t_min, self.t_max) + + def __len__(self) -> int: + return len(self._py_quaternions) + + def __str__(self) -> str: + method = self._method_str + count = len(self._py_quaternions) + if count == 0: + return f"QuaternionSpline(empty, method={method})" + return ( + f"QuaternionSpline({count} points, " + f"t=[{self.t_min:.3f}, {self.t_max:.3f}], method={method})" + ) + + def __repr__(self) -> str: + return self.__str__() + + +class SquadC2(_CppSquadC2): # type: ignore[valid-type, misc] + """C++-backed SquadC2 returning Python Quaternions.""" + + def __init__( + self, + time_points: list[float], + quaternions: list[_PyQuaternion], + normalize_quaternions: bool = True, + validate_continuity: bool = True, + ) -> None: + cpp_quats = [_py_to_cpp(q) for q in quaternions] + super().__init__(time_points, cpp_quats, normalize_quaternions, validate_continuity) + self._n_original = len(quaternions) + + def evaluate(self, t: float) -> _PyQuaternion: + return _cpp_to_py(super().evaluate(t)) + + def get_time_range(self) -> tuple[float, float]: + """Return (t_min, t_max).""" + return (self.t_min, self.t_max) + + def __len__(self) -> int: + return self._n_original + + def __str__(self) -> str: + t_min, t_max = self.get_time_range() + return ( + f"SquadC2({self._n_original} original waypoints, " + f"t=[{t_min:.3f}, {t_max:.3f}])" + ) + + def __repr__(self) -> str: + return self.__str__() + + +class LogQuaternionInterpolation(_CppLogQuaternionInterpolation): # type: ignore[valid-type, misc] + """C++-backed LogQuaternionInterpolation returning Python Quaternions.""" + + def __init__( + self, + time_points: list[float], + quaternions: list[_PyQuaternion], + degree: int = 3, + initial_velocity: object = None, + final_velocity: object = None, + ) -> None: + cpp_quats = [_py_to_cpp(q) for q in quaternions] + super().__init__(time_points, cpp_quats, degree, initial_velocity, final_velocity) + + def evaluate(self, t: float) -> _PyQuaternion: + return _cpp_to_py(super().evaluate(t)) + + +class ModifiedLogQuaternionInterpolation(_CppModifiedLogQuaternionInterpolation): # type: ignore[valid-type, misc] + """C++-backed ModifiedLogQuaternionInterpolation returning Python Quaternions.""" + + def __init__( # noqa: PLR0913 + self, + time_points: list[float], + quaternions: list[_PyQuaternion], + degree: int = 3, + normalize_axis: bool = True, + initial_velocity: object = None, + final_velocity: object = None, + ) -> None: + cpp_quats = [_py_to_cpp(q) for q in quaternions] + super().__init__( + time_points, cpp_quats, degree, normalize_axis, initial_velocity, final_velocity + ) + + def evaluate(self, t: float) -> _PyQuaternion: + return _cpp_to_py(super().evaluate(t)) diff --git a/interpolatepy/_adapters/_spline.py b/interpolatepy/_adapters/_spline.py new file mode 100644 index 0000000..899cd8d --- /dev/null +++ b/interpolatepy/_adapters/_spline.py @@ -0,0 +1,68 @@ +"""Adapters for the cubic-spline family. + +Each class subclasses the C++ pybind11 class and adds Python-only convenience +methods (``plot()``) and property aliases where attribute names differ between +the Python and C++ implementations. +""" + +from __future__ import annotations + +from typing import Any + +from interpolatepy._backend import get_cpp_module +from interpolatepy.cubic_spline import CubicSpline as _PyCubicSpline + +_cpp = get_cpp_module() + +_CppCubicSpline = _cpp.CubicSpline +_CppCubicSmoothingSpline = _cpp.CubicSmoothingSpline +_CppCubicSplineWithAcc1 = _cpp.CubicSplineWithAcceleration1 +_CppCubicSplineWithAcc2 = _cpp.CubicSplineWithAcceleration2 + + +class CubicSpline(_CppCubicSpline): # type: ignore[valid-type, misc] + """C++-backed CubicSpline with Python ``plot()`` method.""" + + plot = _PyCubicSpline.plot + + @property + def n(self) -> int: + """Number of polynomial segments (alias for ``n_segments``).""" + return self.n_segments + + +class CubicSmoothingSpline(_CppCubicSmoothingSpline): # type: ignore[valid-type, misc] + """C++-backed CubicSmoothingSpline with Python property aliases.""" + + @property + def t(self) -> Any: + """Alias mapping Python ``t`` to C++ ``t_points``.""" + return self.t_points + + @property + def q(self) -> Any: + """Alias mapping Python ``q`` to C++ ``q_points``.""" + return self.q_points + + @property + def s(self) -> Any: + """Alias mapping Python ``s`` to C++ ``s_points``.""" + return self.s_points + + +class CubicSplineWithAcceleration1(_CppCubicSplineWithAcc1): # type: ignore[valid-type, misc] + """C++-backed CubicSplineWithAcceleration1 with property aliases.""" + + @property + def t(self) -> Any: + """Alias mapping Python ``t`` to C++ ``t_points``.""" + return self.t_points + + @property + def q(self) -> Any: + """Alias mapping Python ``q`` to C++ ``q_points``.""" + return self.q_points + + +class CubicSplineWithAcceleration2(_CppCubicSplineWithAcc2): # type: ignore[valid-type, misc] + """C++-backed CubicSplineWithAcceleration2 with property aliases.""" diff --git a/interpolatepy/_api.py b/interpolatepy/_api.py new file mode 100644 index 0000000..b8af12f --- /dev/null +++ b/interpolatepy/_api.py @@ -0,0 +1,133 @@ +"""Resolve the active backend and export the public API symbols. + +This module contains the conditional import logic that selects between +C++-backed adapters and pure-Python implementations based on the +``HAS_CPP`` flag from ``_backend``. +""" + +from __future__ import annotations + +from ._backend import HAS_CPP + +if HAS_CPP: + # โ”€โ”€ C++-backed classes and functions โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ + from ._adapters import ( + # Spline + CubicSpline, + CubicSmoothingSpline, + CubicSplineWithAcceleration1, + CubicSplineWithAcceleration2, + SplineParameters, + SplineConfig, + # B-spline + BSpline, + ApproximationBSpline, + CubicBSplineInterpolation, + BSplineInterpolator, + BSplineParams, + SmoothingCubicBSpline, + # Motion profiles + DoubleSTrajectory, + StateParams, + TrajectoryBounds, + BoundaryCondition, + PolynomialTrajectory, + TimeInterval, + TrapezoidalTrajectory, + ParabolicBlendTrajectory, + # Paths + CircularPath, + LinearPath, + # Quaternion + QuaternionSpline, + SquadC2, + LogQuaternionInterpolation, + ModifiedLogQuaternionInterpolation, + # Free functions + solve_tridiagonal, + smoothing_spline_with_tolerance, + linear_traj, + compute_trajectory_frames, + circular_trajectory_with_derivatives, + helicoidal_trajectory_with_derivatives, + ) +else: + # โ”€โ”€ Pure-Python fallback โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€โ”€ + # Core spline algorithms + from .cubic_spline import CubicSpline # type: ignore[assignment] + from .c_s_smoothing import CubicSmoothingSpline # type: ignore[assignment] + from .c_s_smoot_search import SplineConfig + from .c_s_smoot_search import smoothing_spline_with_tolerance + from .c_s_with_acc1 import CubicSplineWithAcceleration1 # type: ignore[assignment] + from .c_s_with_acc2 import CubicSplineWithAcceleration2 # type: ignore[assignment] + from .c_s_with_acc2 import SplineParameters + + # B-spline family + from .b_spline import BSpline # type: ignore[assignment] + from .b_spline_approx import ApproximationBSpline # type: ignore[assignment] + from .b_spline_cubic import CubicBSplineInterpolation # type: ignore[assignment] + from .b_spline_interpolate import BSplineInterpolator # type: ignore[assignment] + from .b_spline_smooth import BSplineParams + from .b_spline_smooth import SmoothingCubicBSpline # type: ignore[assignment] + + # Motion profiles + from .double_s import DoubleSTrajectory # type: ignore[assignment] + from .double_s import StateParams + from .double_s import TrajectoryBounds + from .polynomials import BoundaryCondition + from .polynomials import PolynomialTrajectory # type: ignore[assignment] + from .polynomials import TimeInterval + from .trapezoidal import TrapezoidalTrajectory # type: ignore[assignment] + + # Path planning + from .simple_paths import CircularPath # type: ignore[assignment] + from .simple_paths import LinearPath # type: ignore[assignment] + from .lin_poly_parabolic import ParabolicBlendTrajectory # type: ignore[assignment] + + # Quaternion interpolation + from .quat_spline import QuaternionSpline # type: ignore[assignment] + from .squad_c2 import SquadC2 # type: ignore[assignment] + from .log_quat import LogQuaternionInterpolation # type: ignore[assignment] + from .log_quat import ModifiedLogQuaternionInterpolation # type: ignore[assignment] + + # Free functions + from .tridiagonal_inv import solve_tridiagonal + from .linear import linear_traj + from .frenet_frame import compute_trajectory_frames + from .frenet_frame import circular_trajectory_with_derivatives + from .frenet_frame import helicoidal_trajectory_with_derivatives + +__all__ = [ + "ApproximationBSpline", + "BSpline", + "BSplineInterpolator", + "BSplineParams", + "BoundaryCondition", + "CircularPath", + "CubicBSplineInterpolation", + "CubicSmoothingSpline", + "CubicSpline", + "CubicSplineWithAcceleration1", + "CubicSplineWithAcceleration2", + "DoubleSTrajectory", + "LinearPath", + "LogQuaternionInterpolation", + "ModifiedLogQuaternionInterpolation", + "ParabolicBlendTrajectory", + "PolynomialTrajectory", + "QuaternionSpline", + "SmoothingCubicBSpline", + "SplineConfig", + "SplineParameters", + "SquadC2", + "StateParams", + "TimeInterval", + "TrajectoryBounds", + "TrapezoidalTrajectory", + "circular_trajectory_with_derivatives", + "compute_trajectory_frames", + "helicoidal_trajectory_with_derivatives", + "linear_traj", + "smoothing_spline_with_tolerance", + "solve_tridiagonal", +] diff --git a/interpolatepy/_backend.py b/interpolatepy/_backend.py new file mode 100644 index 0000000..fd56139 --- /dev/null +++ b/interpolatepy/_backend.py @@ -0,0 +1,36 @@ +"""Backend detection for optional C++ acceleration. + +When the compiled C++ extension ``interpolatecpp_py`` is available inside the +package directory, the library transparently delegates to it for faster +evaluation. Set the environment variable ``INTERPOLATEPY_NO_CPP=1`` to force +the pure-Python fallback even when the extension is present. +""" + +from __future__ import annotations + +import importlib +import os +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from types import ModuleType + +_cpp: ModuleType | None = None +HAS_CPP: bool = False + + +def get_cpp_module() -> ModuleType: + """Return the C++ extension module, raising if unavailable.""" + if _cpp is None: + msg = "C++ backend not available" + raise ImportError(msg) + return _cpp + + +if not os.environ.get("INTERPOLATEPY_NO_CPP"): + try: + # The .so lives next to this file inside the interpolatepy package. + _cpp = importlib.import_module(".interpolatecpp_py", package=__package__) + HAS_CPP = True + except ImportError: + pass diff --git a/interpolatepy/c_s_smoothing.py b/interpolatepy/c_s_smoothing.py index 3d5f321..2567869 100644 --- a/interpolatepy/c_s_smoothing.py +++ b/interpolatepy/c_s_smoothing.py @@ -281,7 +281,7 @@ def _solve_system(self) -> np.ndarray: n = self.n # For pure interpolation (ฮผ = 1): Aฯ‰ = c (equation 4.22) - if self.mu == 1.0: + if self.mu == 1.0: # noqa: RUF069 โ€” mu is user-provided, not computed # Construct the vector c according to equation (4.24) c = np.zeros(n) @@ -366,7 +366,7 @@ def _compute_positions(self) -> np.ndarray: For smoothing spline (0<ฮผ<1): s = q - ฮปWโปยนCแต€ฯ‰ (equation 4.36) """ # For pure interpolation (ฮผ = 1), s = q (exact fit) - if self.mu == 1.0: + if self.mu == 1.0: # noqa: RUF069 โ€” mu is user-provided, not computed return self.q.copy() # For smoothing spline (0 < ฮผ < 1), s = q - ฮปWโปยนCแต€ฯ‰ (equation 4.36) diff --git a/interpolatepy/double_s.py b/interpolatepy/double_s.py index b67edb1..73f1094 100644 --- a/interpolatepy/double_s.py +++ b/interpolatepy/double_s.py @@ -365,25 +365,25 @@ def _plan_trajectory(self) -> None: # Round final time to discrete ticks (in milliseconds) self.T = round(self.T * 1000) / 1000 - def evaluate(self, t: float | np.ndarray) -> tuple[float | np.ndarray, ...]: + def evaluate_full(self, t: float | np.ndarray) -> tuple[float | np.ndarray, ...]: """ - Evaluate the double-S trajectory at time t. + Evaluate all trajectory components at time t. - Parameters: - ----------- + Parameters + ---------- t : float or ndarray - Time(s) at which to evaluate the trajectory + Time(s) at which to evaluate the trajectory. - Returns: - -------- + Returns + ------- q : float or ndarray - Position at time t + Position at time t. qp : float or ndarray - Velocity at time t + Velocity at time t. qpp : float or ndarray - Acceleration at time t + Acceleration at time t. qppp : float or ndarray - Jerk at time t + Jerk at time t. """ # Handle array input if isinstance(t, list | np.ndarray): @@ -395,7 +395,7 @@ def evaluate(self, t: float | np.ndarray) -> tuple[float | np.ndarray, ...]: # Compute for each time point for i, t_i in enumerate(t): - q[i], qp[i], qpp[i], qppp[i] = self.evaluate(float(t_i)) + q[i], qp[i], qpp[i], qppp[i] = self.evaluate_full(float(t_i)) return q, qp, qpp, qppp # Special case for equal positions with equal velocities @@ -527,6 +527,74 @@ def evaluate(self, t: float | np.ndarray) -> tuple[float | np.ndarray, ...]: return q_final, qp_final, qpp_final, qppp_final + def evaluate(self, t: float | np.ndarray) -> float | np.ndarray: + """ + Evaluate position at time t. + + Parameters + ---------- + t : float or ndarray + Time(s) at which to evaluate. + + Returns + ------- + float or ndarray + Position at time t. + """ + q, _, _, _ = self.evaluate_full(t) + return q + + def evaluate_velocity(self, t: float | np.ndarray) -> float | np.ndarray: + """ + Evaluate velocity at time t. + + Parameters + ---------- + t : float or ndarray + Time(s) at which to evaluate. + + Returns + ------- + float or ndarray + Velocity at time t. + """ + _, v, _, _ = self.evaluate_full(t) + return v + + def evaluate_acceleration(self, t: float | np.ndarray) -> float | np.ndarray: + """ + Evaluate acceleration at time t. + + Parameters + ---------- + t : float or ndarray + Time(s) at which to evaluate. + + Returns + ------- + float or ndarray + Acceleration at time t. + """ + _, _, a, _ = self.evaluate_full(t) + return a + + def evaluate_jerk(self, t: float | np.ndarray) -> float | np.ndarray: + """ + Evaluate jerk at time t. + + Parameters + ---------- + t : float or ndarray + Time(s) at which to evaluate. + + Returns + ------- + float or ndarray + Jerk at time t. + """ + _, _, _, j = self.evaluate_full(t) + return j + def get_duration(self) -> float: """ Returns the total duration of the trajectory. @@ -582,6 +650,6 @@ def create_trajectory( planner = DoubleSTrajectory(state_params, bounds) def trajectory(t: float | np.ndarray) -> tuple[float | np.ndarray, ...]: - return planner.evaluate(t) + return planner.evaluate_full(t) return trajectory, planner.get_duration() diff --git a/interpolatepy/protocols.py b/interpolatepy/protocols.py new file mode 100644 index 0000000..f5ac932 --- /dev/null +++ b/interpolatepy/protocols.py @@ -0,0 +1,116 @@ +"""Protocol definitions for InterpolatePy trajectory and curve interfaces. + +Defines structural typing protocols (PEP 544) that group classes by behavior +without requiring inheritance. Classes conform by having the right method +signatures โ€” no modifications to existing classes needed. + +Five protocol groups: +- ScalarTrajectory: 1D position/velocity/acceleration evaluation +- CurveEvaluator: Parametric curve with derivative evaluation +- GeometricPath: 3D path with position/velocity/acceleration by arc length +- QuaternionTrajectory: Quaternion-valued trajectory evaluation +- TrajectoryFunction: Callable returning (position, velocity, acceleration) tuple +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Protocol, runtime_checkable + +if TYPE_CHECKING: + import numpy as np + + from .quat_core import Quaternion + + +@runtime_checkable +class ScalarTrajectory(Protocol): + """Protocol for scalar (1D) trajectory evaluation. + + Conforming classes provide position, velocity, and acceleration + as functions of time. Used by spline-based trajectory planners. + + Conforming Classes + ------------------ + CubicSpline, CubicSplineWithAcceleration1, CubicSplineWithAcceleration2, + CubicSmoothingSpline, DoubleSTrajectory + """ + + def evaluate(self, t: float | np.ndarray) -> float | np.ndarray: ... + + def evaluate_velocity(self, t: float | np.ndarray) -> float | np.ndarray: ... + + def evaluate_acceleration(self, t: float | np.ndarray) -> float | np.ndarray: ... + + +@runtime_checkable +class CurveEvaluator(Protocol): + """Protocol for parametric curve evaluation with derivative support. + + Conforming classes evaluate a curve at parameter u and compute + derivatives of arbitrary order. + + Conforming Classes + ------------------ + BSpline, BSplineInterpolator, CubicBSplineInterpolation, + SmoothingCubicBSpline, ApproximationBSpline + """ + + def evaluate(self, u: float) -> np.ndarray: ... + + def evaluate_derivative(self, u: float, order: int = 1) -> np.ndarray: ... + + +@runtime_checkable +class GeometricPath(Protocol): + """Protocol for 3D geometric path evaluation by arc length. + + Conforming classes provide position, velocity, and acceleration + vectors as functions of arc length parameter s. + + Conforming Classes + ------------------ + LinearPath, CircularPath + """ + + def position(self, s: float | np.ndarray) -> np.ndarray: ... + + def velocity(self, s: float | np.ndarray) -> np.ndarray: ... + + def acceleration(self, s: float | np.ndarray) -> np.ndarray: ... + + +@runtime_checkable +class QuaternionTrajectory(Protocol): + """Protocol for quaternion-valued trajectory evaluation. + + Conforming classes provide quaternion interpolation with angular + velocity and acceleration as functions of time. + + Conforming Classes + ------------------ + SquadC2, LogQuaternionInterpolation, ModifiedLogQuaternionInterpolation, + QuaternionSpline + """ + + def evaluate(self, t: float) -> Quaternion: ... + + def evaluate_velocity(self, t: float) -> np.ndarray: ... + + def evaluate_acceleration(self, t: float) -> np.ndarray: ... + + +@runtime_checkable +class TrajectoryFunction(Protocol): + """Protocol for callable trajectory functions returning (pos, vel, acc). + + Conforming objects are callables (typically returned by factory methods) + that accept a time parameter and return a tuple of position, velocity, + and acceleration. + + Conforming Classes + ------------------ + Output callables from TrapezoidalTrajectory, ParabolicBlendTrajectory, + PolynomialTrajectory factories + """ + + def __call__(self, t: float) -> tuple[float, float, float]: ... diff --git a/interpolatepy/quat_spline.py b/interpolatepy/quat_spline.py index ec8c63e..f8a9750 100644 --- a/interpolatepy/quat_spline.py +++ b/interpolatepy/quat_spline.py @@ -321,6 +321,120 @@ def get_quaternion_at_time(self, t: float) -> Quaternion: raise ValueError(f"Interpolation failed with status {status}") return result + def evaluate(self, t: float) -> Quaternion: + """ + Evaluate the quaternion at time t. + + Wrapper around ``interpolate_at_time`` that returns only the quaternion + and raises on error, conforming to the ``QuaternionTrajectory`` protocol. + + Parameters + ---------- + t : float + Time at which to evaluate. + + Returns + ------- + Quaternion + Interpolated quaternion at time t. + + Raises + ------ + ValueError + If the spline is empty or time is out of range. + """ + result, status = self.interpolate_at_time(t) + if status != 0: + if status == -1: + raise ValueError("Spline is empty") + if status == INTERPOLATION_OUT_OF_RANGE_ERROR: + raise ValueError(f"Time {t} is out of range") + raise ValueError(f"Interpolation failed with status {status}") + return result + + def evaluate_velocity(self, t: float) -> np.ndarray: + """ + Evaluate the angular velocity at time t. + + Uses ``interpolate_with_velocity`` internally and returns only the + angular velocity vector, conforming to the ``QuaternionTrajectory`` protocol. + + Parameters + ---------- + t : float + Time at which to evaluate. + + Returns + ------- + np.ndarray + 3D angular velocity vector. + + Raises + ------ + ValueError + If the spline is empty or time is out of range. + """ + _, omega, status = self.interpolate_with_velocity(t) + if status != 0: + if status == -1: + raise ValueError("Spline is empty") + if status == INTERPOLATION_OUT_OF_RANGE_ERROR: + raise ValueError(f"Time {t} is out of range") + raise ValueError(f"Interpolation failed with status {status}") + return omega + + def evaluate_acceleration(self, t: float) -> np.ndarray: + """ + Evaluate the angular acceleration at time t using finite differences. + + Computes a central-difference approximation of angular acceleration + from the angular velocity, conforming to the ``QuaternionTrajectory`` protocol. + + Parameters + ---------- + t : float + Time at which to evaluate. + + Returns + ------- + np.ndarray + 3D angular acceleration vector (finite-difference approximation). + + Raises + ------ + ValueError + If the spline is empty or time is out of range. + + Notes + ----- + This is a numerical approximation using finite differences with + step size dt=1e-6. For applications requiring exact angular + acceleration, consider using ``SquadC2`` or log-quaternion methods. + """ + dt = 1e-6 + times = list(self.quat_data.keys()) + + if not times: + raise ValueError("Spline is empty") + + t_min, t_max = times[0], times[-1] + + # Central difference when possible, forward/backward at boundaries + if t - dt >= t_min and t + dt <= t_max: + w_forward = self.evaluate_velocity(t + dt) + w_backward = self.evaluate_velocity(t - dt) + return (w_forward - w_backward) / (2.0 * dt) + if t + dt <= t_max: + w_now = self.evaluate_velocity(t) + w_forward = self.evaluate_velocity(t + dt) + return (w_forward - w_now) / dt + if t - dt >= t_min: + w_now = self.evaluate_velocity(t) + w_backward = self.evaluate_velocity(t - dt) + return (w_now - w_backward) / dt + + return np.zeros(3) + def get_time_points(self) -> list[float]: """Get all time points in the spline""" return list(self.quat_data.keys()) diff --git a/interpolatepy/simple_paths.py b/interpolatepy/simple_paths.py index b6775cb..c4754c4 100644 --- a/interpolatepy/simple_paths.py +++ b/interpolatepy/simple_paths.py @@ -110,8 +110,7 @@ def velocity(self, _s: float | None = None) -> np.ndarray: # Equation 4.35: dp/ds = (pf-pi)/||pf-pi|| return self.tangent - @staticmethod - def acceleration(_s: float | None = None) -> np.ndarray: + def acceleration(self, _s: float | None = None) -> np.ndarray: # noqa: PLR6301 """ Calculate second derivative with respect to arc length. For linear path, this is always zero. diff --git a/interpolatepy/version.py b/interpolatepy/version.py index f49e11c..b873831 100644 --- a/interpolatepy/version.py +++ b/interpolatepy/version.py @@ -2,4 +2,4 @@ __version__ file. """ -__version__ = "2.0.1" +__version__ = "3.0.0" diff --git a/mkdocs.yml b/mkdocs.yml index 2ef92e5..f7546c9 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -146,8 +146,11 @@ nav: - Tutorials: - Spline Interpolation: tutorials/spline-interpolation.md - Motion Profiles: tutorials/motion-profiles.md + - Quaternion Interpolation: tutorials/quaternion-interpolation.md + - Path Planning: tutorials/path-planning.md - Reference: - API Reference: api-reference.md + - Architecture: architecture.md - Algorithms: algorithms.md - Examples: examples.md - Troubleshooting: troubleshooting.md diff --git a/pyproject.toml b/pyproject.toml index f38ca2d..a2f6f68 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,7 +29,6 @@ classifiers = [ "Intended Audience :: Developers", "Intended Audience :: Education", "Programming Language :: Python :: 3", - "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: 3.13", @@ -43,6 +42,7 @@ classifiers = [ "Topic :: Scientific/Engineering :: Visualization", "Topic :: Software Development :: Libraries", "Topic :: Software Development :: Libraries :: Python Modules", + "Programming Language :: C++", ] dependencies = ["numpy>=2.3.0", "matplotlib>=3.10.5", "scipy>=1.16.0"] dynamic = ["version"] @@ -67,11 +67,23 @@ dev = [ "build>=1.0.3", "twine>=4.0.2", ] -all = ["interpolatepy[test,dev]"] +docs = [ + "mkdocs>=1.5.3", + "mkdocs-material>=9.4.8", + "pymdown-extensions>=10.3.0", + "mkdocs-autorefs>=0.5.0", + "mkdocstrings>=0.23.0", + "mkdocstrings-python>=1.7.3", + "Pygments>=2.16.1", +] +all = ["interpolatepy[test,dev,docs]"] [tool.setuptools] platforms = ["unix", "linux", "osx", "cygwin", "win32"] -packages = ["interpolatepy"] + +[tool.setuptools.packages.find] +where = ["."] +include = ["interpolatepy*"] [tool.setuptools.dynamic] version = { attr = "interpolatepy.version.__version__" } @@ -83,6 +95,7 @@ testpaths = "tests" [tool.ruff] target-version = "py312" line-length = 120 +preview = true extend-exclude = ["docs", "test", "tests"] [tool.ruff.lint] @@ -200,28 +213,7 @@ dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" force-single-line = true force-sort-within-sections = false lines-after-imports = 2 -known-third-party = [ - "numpy", - "pandas", - "keras", - "tensorflow", - "sklearn", - "matplotlib", - "scipy", - "h5py", - "seaborn", - "numba", - "gym", - "PyQt5", - "PyQt6", - "pyqtgraph", - "torch", - "tqdm", - "cv2", - "skimage", - "tensorcross", - "tensorflow_datasets", -] +known-third-party = ["numpy", "matplotlib", "scipy"] split-on-trailing-comma = true combine-as-imports = true force-wrap-aliases = true diff --git a/requirements-dev.txt b/requirements-dev.txt deleted file mode 100644 index 36243f6..0000000 --- a/requirements-dev.txt +++ /dev/null @@ -1,28 +0,0 @@ -# Runtime requirements ---requirement requirements.txt - -# Build and packaging -setuptools>=68.2.2 -build>=1.0.3 -twine>=4.0.2 - -# Linting and type checking (ruff now handles formatting, so no black/isort needed) -ruff>=0.1.5 -mypy>=1.6.1 -pyright>=1.1.335 -pre-commit>=4.1.0 - -# Testing -pytest>=7.4.3 -pytest-cov>=4.1.0 -pytest-benchmark>=4.0.0 -codecov>=2.1.13 - -# Documentation (optional, only if needed) -mkdocs>=1.5.3 -mkdocs-material>=9.4.8 -pymdown-extensions>=10.3.0 -mkdocs-autorefs>=0.5.0 -mkdocstrings>=0.23.0 -mkdocstrings-python>=1.7.3 -Pygments>=2.16.1 diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index c08b4be..0000000 --- a/requirements.txt +++ /dev/null @@ -1,4 +0,0 @@ -# Runtime requirements -numpy>=2.0.0 -matplotlib>=3.10.1 -scipy>=1.15.2 diff --git a/tests/test_backend.py b/tests/test_backend.py new file mode 100644 index 0000000..db3148f --- /dev/null +++ b/tests/test_backend.py @@ -0,0 +1,151 @@ +"""Tests for C++ backend detection and switching.""" + +# ruff: noqa: PLC0415 +from __future__ import annotations + +import os +import subprocess +import sys + +import numpy as np +import numpy.testing as npt +import pytest + + +class TestBackendDetection: + """Tests that the backend detection mechanism works correctly.""" + + def test_has_cpp_flag_is_bool(self) -> None: + from interpolatepy._backend import HAS_CPP + + assert isinstance(HAS_CPP, bool) + + def test_has_cpp_exposed_in_init(self) -> None: + import interpolatepy + + assert hasattr(interpolatepy, "HAS_CPP") + assert isinstance(interpolatepy.HAS_CPP, bool) + + def test_env_var_forces_pure_python(self) -> None: + """INTERPOLATEPY_NO_CPP=1 should force pure-Python mode.""" + result = subprocess.run( + [ + sys.executable, + "-c", + "import interpolatepy; print(interpolatepy.HAS_CPP)", + ], + capture_output=True, + text=True, + env={**os.environ, "INTERPOLATEPY_NO_CPP": "1"}, + check=True, + ) + assert result.stdout.strip() == "False" + + def test_cpp_backend_active_without_env_var(self) -> None: + """Without env var, C++ backend should be active if .so is present.""" + result = subprocess.run( + [ + sys.executable, + "-c", + "import interpolatepy; print(interpolatepy.HAS_CPP)", + ], + capture_output=True, + text=True, + env={k: v for k, v in os.environ.items() if k != "INTERPOLATEPY_NO_CPP"}, + check=True, + ) + # Just check it returns a valid bool string + assert result.stdout.strip() in ("True", "False") + + +class TestCppClassTypes: + """Verify C++ backend classes are used when HAS_CPP is True.""" + + def test_cubic_spline_is_cpp_backed(self) -> None: + import interpolatepy + + if not interpolatepy.HAS_CPP: + pytest.skip("C++ backend not available") + assert "._adapters." in str(interpolatepy.CubicSpline) + + def test_bspline_is_cpp_backed(self) -> None: + import interpolatepy + + if not interpolatepy.HAS_CPP: + pytest.skip("C++ backend not available") + assert "._adapters." in str(interpolatepy.BSpline) + + def test_quaternion_stays_pure_python(self) -> None: + """Quaternion should always be the pure-Python class.""" + import interpolatepy + + assert "quat_core" in str(interpolatepy.Quaternion) + + +class TestNumericalEquivalence: + """Compare Python and C++ results for key algorithms.""" + + @pytest.fixture + def t_points(self) -> list[float]: + return [0.0, 1.0, 2.0, 3.0, 4.0] + + @pytest.fixture + def q_points(self) -> list[float]: + return [0.0, 1.0, 0.5, 2.0, 1.5] + + def test_cubic_spline_evaluate_matches( + self, t_points: list[float], q_points: list[float] + ) -> None: + """C++ and Python CubicSpline should produce identical results.""" + from interpolatepy.cubic_spline import CubicSpline as PyCubicSpline + + import interpolatepy + + if not interpolatepy.HAS_CPP: + pytest.skip("C++ backend not available") + + py_spline = PyCubicSpline(t_points, q_points, v0=0.0, vn=0.0) + cpp_spline = interpolatepy.CubicSpline(t_points, q_points, v0=0.0, vn=0.0) + + t_eval = np.linspace(0.0, 4.0, 50) + npt.assert_allclose( + cpp_spline.evaluate(t_eval), + py_spline.evaluate(t_eval), + rtol=1e-12, + ) + npt.assert_allclose( + cpp_spline.evaluate_velocity(t_eval), + py_spline.evaluate_velocity(t_eval), + rtol=1e-12, + ) + npt.assert_allclose( + cpp_spline.evaluate_acceleration(t_eval), + py_spline.evaluate_acceleration(t_eval), + rtol=1e-10, + ) + + def test_double_s_evaluate_matches(self) -> None: + """C++ and Python DoubleSTrajectory should produce matching results.""" + from interpolatepy.double_s import DoubleSTrajectory as PyDoubleSTrajectory + from interpolatepy.double_s import StateParams as PyStateParams + from interpolatepy.double_s import TrajectoryBounds as PyTrajectoryBounds + + import interpolatepy + + if not interpolatepy.HAS_CPP: + pytest.skip("C++ backend not available") + + py_state = PyStateParams(q_0=0.0, q_1=10.0, v_0=0.0, v_1=0.0) + py_bounds = PyTrajectoryBounds(v_bound=5.0, a_bound=10.0, j_bound=30.0) + py_traj = PyDoubleSTrajectory(py_state, py_bounds) + + cpp_state = interpolatepy.StateParams(q_0=0.0, q_1=10.0, v_0=0.0, v_1=0.0) + cpp_bounds = interpolatepy.TrajectoryBounds(v_bound=5.0, a_bound=10.0, j_bound=30.0) + cpp_traj = interpolatepy.DoubleSTrajectory(cpp_state, cpp_bounds) + + t_eval = np.linspace(0.0, min(py_traj.T, cpp_traj.T), 50) + npt.assert_allclose( + cpp_traj.evaluate(t_eval), + py_traj.evaluate(t_eval), + rtol=1e-10, + ) diff --git a/tests/test_motion_profiles.py b/tests/test_motion_profiles.py index 7dbc148..ef6f0be 100644 --- a/tests/test_motion_profiles.py +++ b/tests/test_motion_profiles.py @@ -150,13 +150,13 @@ def test_evaluate_basic(self) -> None: trajectory = DoubleSTrajectory(state_params, bounds) # Evaluate at start - q, v, a, j = trajectory.evaluate(0.0) + q, v, a, j = trajectory.evaluate_full(0.0) assert abs(q - 0.0) < self.NUMERICAL_ATOL assert abs(v - 0.0) < self.NUMERICAL_ATOL # Evaluate at end duration = trajectory.get_duration() - q, v, a, j = trajectory.evaluate(duration) + q, v, a, j = trajectory.evaluate_full(duration) assert abs(q - 10.0) < self.NUMERICAL_ATOL assert abs(v - 0.0) < self.NUMERICAL_ATOL @@ -169,7 +169,7 @@ def test_evaluate_array_input(self) -> None: # Test with array input t_array = np.array([0.0, 1.0, 2.0]) - q, v, a, j = trajectory.evaluate(t_array) + q, v, a, j = trajectory.evaluate_full(t_array) assert isinstance(q, np.ndarray) assert isinstance(v, np.ndarray) @@ -188,13 +188,13 @@ def test_boundary_conditions(self) -> None: trajectory = DoubleSTrajectory(state_params, bounds) # Check initial conditions - q0, v0, a0, j0 = trajectory.evaluate(0.0) + q0, v0, a0, j0 = trajectory.evaluate_full(0.0) assert abs(q0 - state_params.q_0) < self.NUMERICAL_ATOL assert abs(v0 - state_params.v_0) < self.NUMERICAL_ATOL # Check final conditions duration = trajectory.get_duration() - q1, v1, a1, j1 = trajectory.evaluate(duration) + q1, v1, a1, j1 = trajectory.evaluate_full(duration) assert abs(q1 - state_params.q_1) < self.NUMERICAL_ATOL assert abs(v1 - state_params.v_1) < self.NUMERICAL_ATOL @@ -210,7 +210,7 @@ def test_velocity_bounds_satisfaction(self) -> None: t_samples = np.linspace(0, duration, 100) for t in t_samples: - _, v, _, _ = trajectory.evaluate(t) + _, v, _, _ = trajectory.evaluate_full(t) assert abs(v) <= bounds.v_bound + self.NUMERICAL_ATOL def test_acceleration_bounds_satisfaction(self) -> None: @@ -225,7 +225,7 @@ def test_acceleration_bounds_satisfaction(self) -> None: t_samples = np.linspace(0, duration, 100) for t in t_samples: - _, _, a, _ = trajectory.evaluate(t) + _, _, a, _ = trajectory.evaluate_full(t) assert abs(a) <= bounds.a_bound + self.NUMERICAL_ATOL def test_jerk_bounds_satisfaction(self) -> None: @@ -294,7 +294,7 @@ def test_zero_displacement(self) -> None: assert duration >= 0 # Position should remain constant - q, v, a, j = trajectory.evaluate(duration / 2) + q, v, a, j = trajectory.evaluate_full(duration / 2) assert abs(q - 5.0) < self.NUMERICAL_ATOL def test_small_displacement(self) -> None: @@ -309,7 +309,7 @@ def test_small_displacement(self) -> None: assert duration > 0 # Check final position - q_final, _, _, _ = trajectory.evaluate(duration) + q_final, _, _, _ = trajectory.evaluate_full(duration) assert abs(q_final - 0.001) < self.NUMERICAL_ATOL def test_large_displacement(self) -> None: @@ -324,7 +324,7 @@ def test_large_displacement(self) -> None: assert duration > 0 # Check final position - q_final, _, _, _ = trajectory.evaluate(duration) + q_final, _, _, _ = trajectory.evaluate_full(duration) assert abs(q_final - 1000.0) < self.NUMERICAL_ATOL def test_negative_displacement(self) -> None: @@ -339,7 +339,7 @@ def test_negative_displacement(self) -> None: assert duration > 0 # Check final position - q_final, _, _, _ = trajectory.evaluate(duration) + q_final, _, _, _ = trajectory.evaluate_full(duration) assert abs(q_final - 0.0) < self.NUMERICAL_ATOL def test_non_zero_initial_final_velocities(self) -> None: @@ -350,12 +350,12 @@ def test_non_zero_initial_final_velocities(self) -> None: trajectory = DoubleSTrajectory(state_params, bounds) # Check boundary conditions - q0, v0, _, _ = trajectory.evaluate(0.0) + q0, v0, _, _ = trajectory.evaluate_full(0.0) assert abs(q0 - 0.0) < self.NUMERICAL_ATOL assert abs(v0 - 1.0) < self.NUMERICAL_ATOL duration = trajectory.get_duration() - q1, v1, _, _ = trajectory.evaluate(duration) + q1, v1, _, _ = trajectory.evaluate_full(duration) assert abs(q1 - 10.0) < self.NUMERICAL_ATOL assert abs(v1 - 2.0) < self.NUMERICAL_ATOL @@ -673,7 +673,7 @@ def test_double_s_evaluation_performance( t_values = np.linspace(0, duration, n_evaluations) def evaluate_trajectory() -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - q, v, a, j = trajectory.evaluate(t_values) + q, v, a, j = trajectory.evaluate_full(t_values) return q, v, a, j q, v, a, j = benchmark(evaluate_trajectory) diff --git a/tests/test_protocols.py b/tests/test_protocols.py new file mode 100644 index 0000000..01f4ca4 --- /dev/null +++ b/tests/test_protocols.py @@ -0,0 +1,349 @@ +"""Tests for protocol conformance and functional behavior. + +Verifies that all expected classes satisfy their respective protocol interfaces +using both ``isinstance`` checks (runtime_checkable) and functional invocation. +""" + +from __future__ import annotations + +from collections import OrderedDict + +import numpy as np +import numpy.testing as npt +import pytest + +from interpolatepy import ( + ApproximationBSpline, + BSpline, + BSplineInterpolator, + CircularPath, + CubicBSplineInterpolation, + CubicSmoothingSpline, + CubicSpline, + CubicSplineWithAcceleration1, + CubicSplineWithAcceleration2, + CurveEvaluator, + DoubleSTrajectory, + GeometricPath, + LinearPath, + LogQuaternionInterpolation, + ModifiedLogQuaternionInterpolation, + Quaternion, + QuaternionSpline, + QuaternionTrajectory, + ScalarTrajectory, + SmoothingCubicBSpline, + SquadC2, + StateParams, + TrajectoryBounds, + TrajectoryFunction, +) + + +# --------------------------------------------------------------------------- +# Fixtures +# --------------------------------------------------------------------------- + + +@pytest.fixture() +def cubic_spline() -> CubicSpline: + """Simple cubic spline for testing.""" + return CubicSpline( + t_points=[0.0, 1.0, 2.0, 3.0], + q_points=[0.0, 1.0, 0.5, 2.0], + ) + + +@pytest.fixture() +def double_s_trajectory() -> DoubleSTrajectory: + """Standard double-S trajectory.""" + state = StateParams(q_0=0.0, q_1=10.0, v_0=0.0, v_1=0.0) + bounds = TrajectoryBounds(v_bound=3.0, a_bound=2.0, j_bound=1.0) + return DoubleSTrajectory(state, bounds) + + +@pytest.fixture() +def linear_path() -> LinearPath: + """Linear path from origin to (1, 1, 1).""" + return LinearPath(np.array([0.0, 0.0, 0.0]), np.array([1.0, 1.0, 1.0])) + + +@pytest.fixture() +def circular_path() -> CircularPath: + """Circular path in XY plane.""" + return CircularPath( + r=np.array([0.0, 0.0, 1.0]), + d=np.array([0.0, 0.0, 0.0]), + pi=np.array([1.0, 0.0, 0.0]), + ) + + +@pytest.fixture() +def quaternion_spline() -> QuaternionSpline: + """Quaternion spline with 5 waypoints for SQUAD support.""" + quats = [ + Quaternion.identity(), + Quaternion.from_euler_angles(0.1, 0.0, 0.0), + Quaternion.from_euler_angles(0.2, 0.1, 0.0), + Quaternion.from_euler_angles(0.1, 0.2, 0.1), + Quaternion.identity(), + ] + return QuaternionSpline([0.0, 1.0, 2.0, 3.0, 4.0], quats) + + +@pytest.fixture() +def squad_c2() -> SquadC2: + """SquadC2 interpolator.""" + quats = [ + Quaternion.identity(), + Quaternion.from_euler_angles(0.1, 0.0, 0.0), + Quaternion.from_euler_angles(0.2, 0.1, 0.0), + Quaternion.from_euler_angles(0.1, 0.2, 0.1), + Quaternion.identity(), + ] + return SquadC2([0.0, 1.0, 2.0, 3.0, 4.0], quats) + + +@pytest.fixture() +def log_quat_interp() -> LogQuaternionInterpolation: + """LogQuaternionInterpolation instance.""" + quats = [ + Quaternion.identity(), + Quaternion.from_euler_angles(0.1, 0.0, 0.0), + Quaternion.from_euler_angles(0.2, 0.1, 0.0), + Quaternion.from_euler_angles(0.1, 0.2, 0.1), + Quaternion.identity(), + ] + return LogQuaternionInterpolation([0.0, 1.0, 2.0, 3.0, 4.0], quats) + + +@pytest.fixture() +def modified_log_quat_interp() -> ModifiedLogQuaternionInterpolation: + """ModifiedLogQuaternionInterpolation instance.""" + quats = [ + Quaternion.identity(), + Quaternion.from_euler_angles(0.1, 0.0, 0.0), + Quaternion.from_euler_angles(0.2, 0.1, 0.0), + Quaternion.from_euler_angles(0.1, 0.2, 0.1), + Quaternion.identity(), + ] + return ModifiedLogQuaternionInterpolation([0.0, 1.0, 2.0, 3.0, 4.0], quats) + + +# --------------------------------------------------------------------------- +# ScalarTrajectory conformance +# --------------------------------------------------------------------------- + + +class TestScalarTrajectoryProtocol: + """Tests for ScalarTrajectory protocol conformance.""" + + def test_cubic_spline_isinstance(self, cubic_spline: CubicSpline) -> None: + assert isinstance(cubic_spline, ScalarTrajectory) + + def test_double_s_isinstance(self, double_s_trajectory: DoubleSTrajectory) -> None: + assert isinstance(double_s_trajectory, ScalarTrajectory) + + def test_double_s_evaluate_returns_scalar(self, double_s_trajectory: DoubleSTrajectory) -> None: + """DoubleSTrajectory.evaluate should return position only (not a tuple).""" + result = double_s_trajectory.evaluate(0.0) + assert isinstance(result, float | np.floating) + + def test_double_s_evaluate_full_returns_tuple(self, double_s_trajectory: DoubleSTrajectory) -> None: + """evaluate_full should still return the 4-tuple.""" + result = double_s_trajectory.evaluate_full(0.0) + assert isinstance(result, tuple) + assert len(result) == 4 + + def test_double_s_individual_methods_match_full(self, double_s_trajectory: DoubleSTrajectory) -> None: + """Individual methods should match evaluate_full output.""" + t = 1.0 + q_full, v_full, a_full, j_full = double_s_trajectory.evaluate_full(t) + + npt.assert_allclose(double_s_trajectory.evaluate(t), q_full, rtol=1e-12) + npt.assert_allclose(double_s_trajectory.evaluate_velocity(t), v_full, rtol=1e-12) + npt.assert_allclose(double_s_trajectory.evaluate_acceleration(t), a_full, rtol=1e-12) + npt.assert_allclose(double_s_trajectory.evaluate_jerk(t), j_full, rtol=1e-12) + + def test_cubic_spline_functional(self, cubic_spline: CubicSpline) -> None: + """CubicSpline methods should be callable through protocol.""" + + def use_scalar_trajectory(traj: ScalarTrajectory, t: float) -> tuple[float, float, float]: + pos = traj.evaluate(t) + vel = traj.evaluate_velocity(t) + acc = traj.evaluate_acceleration(t) + return pos, vel, acc + + pos, vel, acc = use_scalar_trajectory(cubic_spline, 1.0) + assert pos is not None + assert vel is not None + assert acc is not None + + def test_negative_isinstance(self, linear_path: LinearPath) -> None: + """LinearPath should NOT satisfy ScalarTrajectory.""" + assert not isinstance(linear_path, ScalarTrajectory) + + +# --------------------------------------------------------------------------- +# CurveEvaluator conformance +# --------------------------------------------------------------------------- + + +class TestCurveEvaluatorProtocol: + """Tests for CurveEvaluator protocol conformance.""" + + def test_bspline_interpolator_isinstance(self) -> None: + """BSplineInterpolator should satisfy CurveEvaluator.""" + points = np.array([[0, 0, 0], [1, 2, 0], [3, 1, 0], [4, 3, 0], [5, 0, 0]], dtype=float) + interp = BSplineInterpolator(degree=3, points=points) + assert isinstance(interp, CurveEvaluator) + + def test_bspline_isinstance(self) -> None: + """BSpline should satisfy CurveEvaluator.""" + control_points = np.array([[0, 0], [1, 2], [3, 1], [4, 0]], dtype=float) + knots = np.array([0, 0, 0, 0, 1, 1, 1, 1], dtype=float) + bspline = BSpline(degree=3, knots=knots, control_points=control_points) + assert isinstance(bspline, CurveEvaluator) + + def test_negative_isinstance(self, cubic_spline: CubicSpline) -> None: + """CubicSpline should NOT satisfy CurveEvaluator (no evaluate_derivative).""" + assert not isinstance(cubic_spline, CurveEvaluator) + + +# --------------------------------------------------------------------------- +# GeometricPath conformance +# --------------------------------------------------------------------------- + + +class TestGeometricPathProtocol: + """Tests for GeometricPath protocol conformance.""" + + def test_linear_path_isinstance(self, linear_path: LinearPath) -> None: + assert isinstance(linear_path, GeometricPath) + + def test_circular_path_isinstance(self, circular_path: CircularPath) -> None: + assert isinstance(circular_path, GeometricPath) + + def test_linear_path_functional(self, linear_path: LinearPath) -> None: + """LinearPath should work through GeometricPath interface.""" + + def evaluate_path(path: GeometricPath, s: float) -> dict[str, np.ndarray]: + return { + "position": path.position(s), + "velocity": path.velocity(s), + "acceleration": path.acceleration(s), + } + + result = evaluate_path(linear_path, 0.5) + assert result["position"].shape == (3,) + assert result["velocity"].shape == (3,) + assert result["acceleration"].shape == (3,) + + def test_circular_path_functional(self, circular_path: CircularPath) -> None: + """CircularPath should work through GeometricPath interface.""" + + def evaluate_path(path: GeometricPath, s: float) -> dict[str, np.ndarray]: + return { + "position": path.position(s), + "velocity": path.velocity(s), + "acceleration": path.acceleration(s), + } + + result = evaluate_path(circular_path, 0.5) + assert result["position"].shape == (3,) + assert result["velocity"].shape == (3,) + assert result["acceleration"].shape == (3,) + + def test_negative_isinstance(self, cubic_spline: CubicSpline) -> None: + """CubicSpline should NOT satisfy GeometricPath.""" + assert not isinstance(cubic_spline, GeometricPath) + + +# --------------------------------------------------------------------------- +# QuaternionTrajectory conformance +# --------------------------------------------------------------------------- + + +class TestQuaternionTrajectoryProtocol: + """Tests for QuaternionTrajectory protocol conformance.""" + + def test_squad_c2_isinstance(self, squad_c2: SquadC2) -> None: + assert isinstance(squad_c2, QuaternionTrajectory) + + def test_log_quat_isinstance(self, log_quat_interp: LogQuaternionInterpolation) -> None: + assert isinstance(log_quat_interp, QuaternionTrajectory) + + def test_modified_log_quat_isinstance( + self, modified_log_quat_interp: ModifiedLogQuaternionInterpolation + ) -> None: + assert isinstance(modified_log_quat_interp, QuaternionTrajectory) + + def test_quaternion_spline_isinstance(self, quaternion_spline: QuaternionSpline) -> None: + assert isinstance(quaternion_spline, QuaternionTrajectory) + + def test_squad_c2_functional(self, squad_c2: SquadC2) -> None: + """SquadC2 should work through QuaternionTrajectory interface.""" + + def use_quat_traj(traj: QuaternionTrajectory, t: float) -> Quaternion: + q = traj.evaluate(t) + vel = traj.evaluate_velocity(t) + acc = traj.evaluate_acceleration(t) + assert isinstance(q, Quaternion) + assert isinstance(vel, np.ndarray) + assert isinstance(acc, np.ndarray) + return q + + q = use_quat_traj(squad_c2, 2.0) + assert isinstance(q, Quaternion) + + def test_quaternion_spline_evaluate(self, quaternion_spline: QuaternionSpline) -> None: + """QuaternionSpline.evaluate should return a Quaternion.""" + q = quaternion_spline.evaluate(2.0) + assert isinstance(q, Quaternion) + + def test_quaternion_spline_evaluate_velocity(self, quaternion_spline: QuaternionSpline) -> None: + """QuaternionSpline.evaluate_velocity should return an ndarray.""" + omega = quaternion_spline.evaluate_velocity(2.0) + assert isinstance(omega, np.ndarray) + assert omega.shape == (3,) + + def test_quaternion_spline_evaluate_acceleration(self, quaternion_spline: QuaternionSpline) -> None: + """QuaternionSpline.evaluate_acceleration should return an ndarray.""" + alpha = quaternion_spline.evaluate_acceleration(2.0) + assert isinstance(alpha, np.ndarray) + assert alpha.shape == (3,) + + def test_quaternion_spline_evaluate_raises_on_empty(self) -> None: + """evaluate should raise on empty spline.""" + spline = QuaternionSpline.__new__(QuaternionSpline) + spline.quat_data = OrderedDict() + spline.interpolation_method = "auto" + spline.intermediate_quaternions = {} + with pytest.raises(ValueError, match="empty"): + spline.evaluate(0.0) + + def test_negative_isinstance(self, linear_path: LinearPath) -> None: + """LinearPath should NOT satisfy QuaternionTrajectory (no evaluate method).""" + assert not isinstance(linear_path, QuaternionTrajectory) + + +# --------------------------------------------------------------------------- +# TrajectoryFunction conformance +# --------------------------------------------------------------------------- + + +class TestTrajectoryFunctionProtocol: + """Tests for TrajectoryFunction protocol conformance.""" + + def test_callable_isinstance(self) -> None: + """A simple callable returning a 3-tuple should satisfy TrajectoryFunction.""" + + def my_traj(t: float) -> tuple[float, float, float]: + return t, 1.0, 0.0 + + assert isinstance(my_traj, TrajectoryFunction) + + def test_lambda_isinstance(self) -> None: + """Lambda should also satisfy TrajectoryFunction.""" + f = lambda t: (t, 0.0, 0.0) # noqa: E731 + assert isinstance(f, TrajectoryFunction)