├── .gitmodules ├── CPPLINT.cfg ├── docs ├── images │ ├── 2dpoints.png │ ├── 3dpoints.png │ ├── boundingbox.png │ └── closestpoint.png └── Doxyfile.in ├── CMakeLists.txt ├── cmake ├── example_utility.cmake ├── googletest-download.cmake ├── matplotlibcpp-download.cmake ├── googletest.cmake ├── matplotlibcpp.cmake └── build_options.cmake ├── tests ├── CMakeLists.txt ├── TestPolynomial1D.cpp └── TestBezier.cpp ├── examples ├── CMakeLists.txt ├── ExApproximateLength.cpp ├── ExPolynomial1D.cpp ├── ExUserDefinedPointType.cpp ├── ExBoundingBox.cpp ├── Ex2DPoints.cpp ├── Ex3DPoints.cpp └── ClosestPointsToCurve.cpp ├── src ├── CMakeLists.txt └── bezier │ └── NumericalMaths.cpp ├── .gitignore ├── .travis.yml ├── include └── bezier │ ├── NumericalMaths.hpp │ ├── PowerBasisPolynomial1D.hpp │ ├── SturmSequence.hpp │ └── Bezier.hpp ├── .clang-format └── README.md /.gitmodules: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /CPPLINT.cfg: -------------------------------------------------------------------------------- 1 | filter=-whitespace/braces,-runtime/references,-build/include_order 2 | linelength=120 3 | -------------------------------------------------------------------------------- /docs/images/2dpoints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fangzheng81/bezier_curve/HEAD/docs/images/2dpoints.png -------------------------------------------------------------------------------- /docs/images/3dpoints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fangzheng81/bezier_curve/HEAD/docs/images/3dpoints.png -------------------------------------------------------------------------------- /docs/images/boundingbox.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fangzheng81/bezier_curve/HEAD/docs/images/boundingbox.png -------------------------------------------------------------------------------- /docs/images/closestpoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fangzheng81/bezier_curve/HEAD/docs/images/closestpoint.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | 3 | project(libbezier_curve ) 4 | 5 | set(PROJECT_TARGET_LIB_NAME "bezier_curve") 6 | 7 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 8 | 9 | include(build_options) 10 | 11 | add_subdirectory(src) 12 | -------------------------------------------------------------------------------- /cmake/example_utility.cmake: -------------------------------------------------------------------------------- 1 | function(__build_example example_name public_lib private_lib) 2 | add_executable( 3 | ${example_name} 4 | ${CMAKE_CURRENT_LIST_DIR}/${example_name}.cpp 5 | ) 6 | 7 | target_link_libraries( 8 | ${example_name} 9 | PUBLIC 10 | ${public_lib} 11 | PRIVATE 12 | ${private_lib} 13 | ) 14 | endfunction() 15 | -------------------------------------------------------------------------------- /cmake/googletest-download.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | 3 | project(googletest-download NONE) 4 | 5 | include(ExternalProject) 6 | 7 | ExternalProject_Add( 8 | googletest 9 | SOURCE_DIR "@GOOGLETEST_DOWNLOAD_ROOT@/googletest-src" 10 | BINARY_DIR "@GOOGLETEST_DOWNLOAD_ROOT@/googletest-build" 11 | GIT_REPOSITORY 12 | https://github.com/google/googletest.git 13 | GIT_TAG 14 | release-1.8.0 15 | CONFIGURE_COMMAND "" 16 | BUILD_COMMAND "" 17 | INSTALL_COMMAND "" 18 | TEST_COMMAND "" 19 | ) 20 | -------------------------------------------------------------------------------- /cmake/matplotlibcpp-download.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | 3 | project(matplotlibcpp-download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add( 7 | matplotlibcpp 8 | SOURCE_DIR "@MATPLOTLIBCPP_DOWNLOAD_ROOT@/matplotlibcpp-src" 9 | BINARY_DIR "@MATPLOTLIBCPP_DOWNLOAD_ROOT@/matplotlibcpp-build" 10 | GIT_REPOSITORY 11 | https://github.com/xmba15/Another_MatplotlibCpp.git 12 | GIT_TAG 13 | master 14 | CONFIGURE_COMMAND "" 15 | BUILD_COMMAND "" 16 | INSTALL_COMMAND "" 17 | TEST_COMMAND "" 18 | ) 19 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if (NOT TARGET gtest) 2 | include(googletest) 3 | __fetch_googletest( 4 | ${PROJECT_SOURCE_DIR}/cmake 5 | ${PROJECT_BINARY_DIR}/${PROJECT_TARGET_LIB_NAME}_googletest 6 | ) 7 | endif() 8 | 9 | add_executable( 10 | ${PROJECT_TARGET_LIB_NAME}_unit_tests 11 | TestBezier.cpp 12 | TestPolynomial1D.cpp 13 | ) 14 | 15 | target_link_libraries( 16 | ${PROJECT_TARGET_LIB_NAME}_unit_tests 17 | PUBLIC 18 | ${PROJECT_TARGET_LIB_NAME} 19 | PRIVATE 20 | gtest_main 21 | ) 22 | 23 | add_test( 24 | NAME 25 | ${PROJECT_TARGET_LIB_NAME}_unit_tests 26 | COMMAND 27 | $ 28 | ) 29 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(matplotlibcpp) 2 | __fetch_matplotlibcpp( 3 | "${PROJECT_SOURCE_DIR}/cmake" 4 | "${PROJECT_BINARY_DIR}/${PROJECT_TARGET_LIB_NAME}_matplotlibcpp" 5 | ) 6 | 7 | list(APPEND EXAMPLES 8 | "Ex2DPoints" 9 | "Ex3DPoints" 10 | "ExUserDefinedPointType" 11 | "ExPolynomial1D" 12 | "ClosestPointsToCurve" 13 | "ExBoundingBox" 14 | "ExApproximateLength" 15 | ) 16 | 17 | include(example_utility) 18 | 19 | list(APPEND PUBLIC_LIBS 20 | ${PROJECT_TARGET_LIB_NAME} 21 | matplotlib_cpp 22 | ) 23 | 24 | list(APPEND PRIVATE_LIBS 25 | ) 26 | 27 | foreach(EXAMPLE ${EXAMPLES}) 28 | __build_example( 29 | ${EXAMPLE} 30 | "${PUBLIC_LIBS}" 31 | "${PRIVATE_LIBS}" 32 | ) 33 | endforeach(EXAMPLE) 34 | -------------------------------------------------------------------------------- /cmake/googletest.cmake: -------------------------------------------------------------------------------- 1 | function(__fetch_googletest download_module_path download_root) 2 | set(GOOGLETEST_DOWNLOAD_ROOT ${download_root}) 3 | configure_file( 4 | ${download_module_path}/googletest-download.cmake 5 | ${download_root}/CMakeLists.txt 6 | @ONLY 7 | ) 8 | unset(GOOGLETEST_DOWNLOAD_ROOT) 9 | 10 | execute_process( 11 | COMMAND 12 | "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . 13 | WORKING_DIRECTORY 14 | ${download_root} 15 | ) 16 | execute_process( 17 | COMMAND 18 | "${CMAKE_COMMAND}" --build . 19 | WORKING_DIRECTORY 20 | ${download_root} 21 | ) 22 | 23 | add_subdirectory( 24 | ${download_root}/googletest-src 25 | ${download_root}/googletest-build 26 | ) 27 | endfunction() 28 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(LIBRARY_NAME "${PROJECT_TARGET_LIB_NAME}") 2 | 3 | file(GLOB SOURCE_FILES 4 | ${CMAKE_CURRENT_LIST_DIR}/bezier/*.cpp 5 | ${CMAKE_CURRENT_LIST_DIR}/bezier/*.hpp 6 | ) 7 | 8 | find_package(Eigen3 REQUIRED NO_MODULE) 9 | 10 | add_library(${LIBRARY_NAME} 11 | SHARED 12 | ${SOURCE_FILES} 13 | ) 14 | 15 | target_include_directories(${LIBRARY_NAME} PUBLIC 16 | $ 17 | $ 18 | ) 19 | 20 | target_compile_features(${LIBRARY_NAME} 21 | PRIVATE 22 | cxx_std_11 23 | ) 24 | 25 | target_compile_options(${LIBRARY_NAME} 26 | PRIVATE 27 | $<$:-O0 -g -Wall -Werror> 28 | $<$:-O3> 29 | ) 30 | 31 | target_link_libraries(${LIBRARY_NAME} 32 | PUBLIC 33 | Eigen3::Eigen 34 | ) 35 | -------------------------------------------------------------------------------- /cmake/matplotlibcpp.cmake: -------------------------------------------------------------------------------- 1 | function(__fetch_matplotlibcpp download_module_path download_root) 2 | set(MATPLOTLIBCPP_DOWNLOAD_ROOT ${download_root}) 3 | configure_file( 4 | ${download_module_path}/matplotlibcpp-download.cmake 5 | ${download_root}/CMakeLists.txt 6 | @ONLY 7 | ) 8 | unset(MATPLOTLIBCPP_DOWNLOAD_ROOT) 9 | 10 | execute_process( 11 | COMMAND 12 | "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . 13 | WORKING_DIRECTORY 14 | ${download_root} 15 | ) 16 | execute_process( 17 | COMMAND 18 | "${CMAKE_COMMAND}" --build . 19 | WORKING_DIRECTORY 20 | ${download_root} 21 | ) 22 | 23 | add_subdirectory( 24 | ${download_root}/matplotlibcpp-src 25 | ${download_root}/matplotlibcpp-build 26 | ) 27 | message(STATUS "${download_root}/matplotlibcpp-src") 28 | endfunction() 29 | -------------------------------------------------------------------------------- /src/bezier/NumericalMaths.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Types.cpp 3 | * 4 | * @brief Implementation of Types 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-19 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include "bezier/NumericalMaths.hpp" 15 | 16 | namespace robotics 17 | { 18 | namespace maths 19 | { 20 | std::vector binomialCoeffs(const size_t n) 21 | { 22 | std::vector results(n + 1, 0); 23 | results.reserve(n + 1); 24 | 25 | size_t k = 0; 26 | size_t center = n / 2; 27 | 28 | for (; k <= center; ++k) { 29 | results[k] = binomialCoeff(n, k); 30 | results[n - k] = results[k]; 31 | } 32 | 33 | return results; 34 | } 35 | 36 | std::vector polynomialCoeffs(const size_t n, const double t) 37 | { 38 | std::vector results; 39 | results.reserve(n + 1); 40 | 41 | for (size_t i = 0; i <= n; ++i) { 42 | results.emplace_back(pow(1 - t, n - i) * pow(t, i)); 43 | } 44 | 45 | return results; 46 | } 47 | 48 | } // namespace maths 49 | } // namespace robotics 50 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.pyc 10 | *~ 11 | *# 12 | build 13 | 14 | # Packages # 15 | ################### 16 | # it's better to unpack these files and commit the raw source 17 | # git has its own built in compression methods 18 | *.7z 19 | *.dmg 20 | *.gz 21 | *.iso 22 | *.jar 23 | *.rar 24 | *.tar 25 | *.zip 26 | 27 | # Logs and databases # 28 | ###################### 29 | *.log 30 | *.sql 31 | *.sqlite 32 | 33 | # OS generated files # 34 | ###################### 35 | .DS_Store 36 | .DS_Store? 37 | ._* 38 | .Spotlight-V100 39 | .Trashes 40 | ehthumbs.db 41 | Thumbs.db 42 | 43 | # Images 44 | ###################### 45 | *.jpg 46 | *.gif 47 | *.png 48 | *.svg 49 | *.ico 50 | 51 | # Video 52 | ###################### 53 | *.wmv 54 | *.mpg 55 | *.mpeg 56 | *.mp4 57 | *.mov 58 | *.flv 59 | *.avi 60 | *.ogv 61 | *.ogg 62 | *.webm 63 | 64 | # Audio 65 | ###################### 66 | *.wav 67 | *.mp3 68 | *.wma 69 | 70 | # Fonts 71 | ###################### 72 | Fonts 73 | *.eot 74 | *.ttf 75 | *.woff 76 | 77 | # Format 78 | ###################### 79 | CPPLINT.cfg 80 | .clang-format 81 | 82 | # Gtags 83 | ###################### 84 | GPATH 85 | GRTAGS 86 | GSYMS 87 | GTAGS 88 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: 2 | - cpp 3 | 4 | dist: xenial 5 | 6 | sudo: false 7 | 8 | compiler: 9 | - gcc 10 | 11 | branches: 12 | only: 13 | - master 14 | 15 | addons: 16 | apt: 17 | sources: 18 | - ubuntu-toolchain-r-test 19 | packages: 20 | - cmake 21 | - g++-6 22 | 23 | env: 24 | - MATRIX_EVAL="CC=gcc-6 && CXX=g++-6" 25 | 26 | before_install: 27 | - readonly PARENT_DIR=$(pwd) 28 | - eval "${MATRIX_EVAL}" 29 | 30 | # build from source latest eigen 31 | - git clone https://github.com/eigenteam/eigen-git-mirror.git 32 | - cd eigen-git-mirror 33 | - mkdir build && cd build && cmake ../ && make && sudo make install 34 | 35 | before_script: 36 | - pyenv versions 37 | - pyenv global 3.6.7 38 | - ls -al $(python-config --prefix)/bin $(python-config --prefix)/lib $(python-config --prefix)/include 39 | 40 | script: 41 | - cd ${PARENT_DIR} 42 | - mkdir build 43 | - cd build 44 | - | 45 | cmake \ 46 | -DWITH_GTEST=ON \ 47 | -DBUILD_EXAMPLES=ON \ 48 | -DPYTHON_EXECUTABLE=$(python-config --prefix)/bin/python3.6 \ 49 | -DPYTHON_LIBRARY=$(python-config --prefix)/lib/libpython3.6m.so \ 50 | -DPYTHON_INCLUDE_DIR=$(python-config --prefix)/include/python3.6m \ 51 | .. 52 | - cmake --build . 53 | - make test 54 | 55 | notifications: 56 | email: false 57 | -------------------------------------------------------------------------------- /examples/ExApproximateLength.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ExApproximateLength.cpp 3 | * 4 | * @author bt 5 | * 6 | * @date 2019-09-20 7 | * 8 | * Copyright (c) organization 9 | * 10 | */ 11 | 12 | #include 13 | 14 | #include 15 | 16 | int main(int argc, char* argv[]) 17 | { 18 | using EigenBezier = robotics::Bezier; 19 | EigenBezier::VecPointType eigenPV{{1, 2, 5}, {2, 6, 3}, {3, 2, 3}, {4, 3, 3}, {5, 13, 5}, 20 | {6, 4, 7}, {7, 1, 9}, {8, 2, 11}, {9, 4, 9}, {10, 8, 10}}; 21 | 22 | EigenBezier bezier(eigenPV); 23 | 24 | double appLength1, appLength2; 25 | 26 | { 27 | auto start = std::chrono::high_resolution_clock::now(); 28 | 29 | EigenBezier::VecPointType trajectory = bezier.trajectory(100); 30 | double appLength2 = bezier.approximateLength(trajectory); 31 | 32 | auto end = std::chrono::high_resolution_clock::now(); 33 | 34 | double executedTime = std::chrono::duration_cast(end - start).count(); 35 | 36 | // change into sec 37 | executedTime *= 1e-9; 38 | 39 | std::cout << "appLength2: " << appLength2 << "\n"; 40 | std::cout << "[2] Time taken by program is : " << std::fixed << executedTime << std::setprecision(6) 41 | << " [sec]\n"; 42 | } 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /docs/Doxyfile.in: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = "Bezier Curve" 2 | 3 | OUTPUT_DIRECTORY = @CMAKE_CURRENT_BINARY_DIR@/doc_doxygen/ 4 | 5 | INPUT = @CMAKE_CURRENT_SOURCE_DIR@/include/bezier/ \ 6 | @CMAKE_CURRENT_SOURCE_DIR@/src/bezier/ \ 7 | @CMAKE_CURRENT_SOURCE_DIR@/docs 8 | 9 | DOXYFILE_ENCODING = UTF-8 10 | 11 | QUIET = YES 12 | 13 | EXTRACT_ALL = NO 14 | 15 | EXTRACT_PRIVATE = YES 16 | 17 | EXTRACT_STATIC = YES 18 | 19 | EXTRACT_LOCAL_CLASSES= YES 20 | 21 | EXTRACT_ANON_NSPACES = YES 22 | 23 | EXTRACT_LOCAL_METHODS= YES 24 | 25 | HIDE_UNDOC_MEMBERS = NO 26 | 27 | HIDE_UNDOC_CLASSES = NO 28 | 29 | HIDE_FRIEND_COMPOUNDS= NO 30 | 31 | HIDE_IN_BODY_DOCS = NO 32 | 33 | CLASS_DIAGRAMS = YES 34 | 35 | HIDE_UNDOC_RELATIONS = NO 36 | 37 | HAVE_DOT = YES 38 | 39 | CLASS_GRAPH = YES 40 | 41 | CALL_GRAPH = YES 42 | 43 | CALLER_GRAPH = YES 44 | 45 | DIRECTORY_GRAPH = YES 46 | 47 | COLLABORATION_GRAPH = YES 48 | 49 | UML_LOOK = YES 50 | 51 | UML_LIMIT_NUM_FIELDS = 50 52 | 53 | TEMPLATE_RELATIONS = YES 54 | 55 | DOT_GRAPH_MAX_NODES = 100 56 | 57 | MAX_DOT_GRAPH_DEPTH = 0 58 | 59 | DOT_TRANSPARENT = YES 60 | 61 | GENERATE_LATEX = NO 62 | 63 | WARN_LOGFILE = @CMAKE_CURRENT_BINARY_DIR@/warn_logfile 64 | -------------------------------------------------------------------------------- /tests/TestPolynomial1D.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file TestPolynomial1D.cpp 3 | * 4 | * @brief test polynomial 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-08-11 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include "gtest/gtest.h" 15 | #include 16 | #include 17 | 18 | class TestPolynomial : public ::testing::Test 19 | { 20 | protected: 21 | void SetUp() override 22 | { 23 | start_time_ = time(nullptr); 24 | } 25 | 26 | void TearDown() override 27 | { 28 | const time_t end_time = time(nullptr); 29 | 30 | // expect test time less than 5 sec 31 | EXPECT_LE(end_time - start_time_, 5); 32 | } 33 | 34 | time_t start_time_; 35 | }; 36 | 37 | TEST_F(TestPolynomial, TestInitialization) 38 | { 39 | using Polynomial = robotics::maths::PowerBasisPolynomial1D; 40 | 41 | { 42 | std::vector coeffs{1.9, 3.4, 5.7}; 43 | Polynomial polynomial(coeffs); 44 | EXPECT_TRUE(polynomial.degree() == coeffs.size() - 1); 45 | } 46 | 47 | { 48 | std::vector coeffs{0.0}; 49 | Polynomial polynomial(coeffs); 50 | EXPECT_TRUE(polynomial.degree() == coeffs.size() - 1); 51 | } 52 | 53 | { 54 | std::vector coeffs{1.0}; 55 | Polynomial polynomial(coeffs); 56 | EXPECT_TRUE(polynomial.degree() == coeffs.size() - 1); 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /cmake/build_options.cmake: -------------------------------------------------------------------------------- 1 | option(BUILD_EXAMPLES "Option whether to build examples" OFF) 2 | if(${BUILD_EXAMPLES}) 3 | message(STATUS "Also build examples") 4 | add_subdirectory(${PROJECT_SOURCE_DIR}/examples) 5 | endif(${BUILD_EXAMPLES}) 6 | 7 | option(WITH_DEBUG "Enable debug" OFF) 8 | if(WITH_DEBUG) 9 | target_compile_definitions(${PROJECT_TARGET_LIB_NAME} 10 | PUBLIC 11 | WITH_DEBUG 12 | ) 13 | endif(WITH_DEBUG) 14 | 15 | option(WITH_GTEST "Enable gtest" OFF) 16 | if(WITH_GTEST) 17 | enable_testing() 18 | add_subdirectory(tests) 19 | endif(WITH_GTEST) 20 | 21 | option(WITH_VISUALIZATION "Option whether to build examples with visualization" OFF) 22 | if(WITH_VISUALIZATION) 23 | target_compile_definitions(${PROJECT_TARGET_LIB_NAME} 24 | PUBLIC 25 | WITH_DEBUG 26 | ) 27 | endif(WITH_VISUALIZATION) 28 | 29 | option(BUILD_DOC "Build documentation" ON) 30 | if(BUILD_DOC) 31 | find_package(Doxygen QUIET) 32 | if(DOXYGEN_FOUND) 33 | set(DOXYGEN_IN ${PROJECT_SOURCE_DIR}/docs/Doxyfile.in) 34 | set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 35 | 36 | configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) 37 | message(STATUS "Doxygen build started") 38 | 39 | add_custom_target(${PROJECT_TARGET_LIB_NAME}_doc_doxygen ALL 40 | COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} 41 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 42 | COMMENT "Generating API documentation with Doxygen" 43 | VERBATIM ) 44 | else() 45 | message(STATUS "Doxygen not found") 46 | endif(DOXYGEN_FOUND) 47 | endif(BUILD_DOC) 48 | -------------------------------------------------------------------------------- /include/bezier/NumericalMaths.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Types.hpp 3 | * 4 | * @brief Types 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-19 9 | * 10 | * Bezier 11 | * 12 | * Copyright (c) organization 13 | * 14 | */ 15 | 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace robotics 26 | { 27 | namespace maths 28 | { 29 | constexpr double FUZZY_EPSILON = 1e-4; 30 | 31 | /** 32 | * @brief function to calculate binomial coefficients 33 | * 34 | * This is a O(k) algorithm. return type of double is used to avoid overflow 35 | * 36 | * @param n for kCn 37 | * @param k for kCn 38 | * @return double 39 | */ 40 | inline double binomialCoeff(const size_t n, const size_t k) 41 | { 42 | if (n < k) { 43 | throw std::out_of_range("n must not be less than k"); 44 | } 45 | 46 | double res = 1.0; 47 | 48 | const size_t optimizedK = (k > n - k) ? n - k : k; 49 | 50 | for (size_t i = 0; i < optimizedK; ++i) { 51 | res = (n - i) * res / (i + 1); 52 | } 53 | 54 | return res; 55 | } 56 | 57 | std::vector binomialCoeffs(const size_t n); 58 | 59 | std::vector polynomialCoeffs(const size_t n, const double t); 60 | 61 | template 62 | bool combinedToleranceEquals(const T& val, const T& correctVal, const T& epsilon = std::numeric_limits::epsilon()) 63 | { 64 | const T maxXYOne = std::max({static_cast(1.0f), std::fabs(val), std::fabs(correctVal)}); 65 | return std::fabs(val - correctVal) <= epsilon * maxXYOne; 66 | } 67 | 68 | template bool isWithinZeroAndOne(const T& x, const T& epsilon = std::numeric_limits::epsilon()) 69 | { 70 | return x >= -epsilon && x <= (static_cast(1.0f) + epsilon); 71 | } 72 | 73 | template constexpr int sgn(const T& a, const T& b) noexcept 74 | { 75 | return (a > b) - (a < b); 76 | } 77 | 78 | template constexpr int sgn(const T& a) noexcept 79 | { 80 | return sgn(a, T(0)); 81 | } 82 | 83 | } // namespace maths 84 | } // namespace robotics 85 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -3 5 | AlignAfterOpenBracket: true 6 | AlignEscapedNewlinesLeft: false 7 | AlignOperands: true 8 | AlignTrailingComments: true 9 | AllowAllParametersOfDeclarationOnNextLine: true 10 | AllowShortBlocksOnASingleLine: false 11 | AllowShortCaseLabelsOnASingleLine: false 12 | AllowShortIfStatementsOnASingleLine: false 13 | AllowShortLoopsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: None 15 | AlwaysBreakAfterDefinitionReturnType: false 16 | AlwaysBreakTemplateDeclarations: false 17 | AlwaysBreakBeforeMultilineStrings: false 18 | BreakBeforeBinaryOperators: None 19 | BreakBeforeTernaryOperators: true 20 | BreakConstructorInitializersBeforeComma: false 21 | BinPackParameters: true 22 | BinPackArguments: true 23 | ColumnLimit: 120 24 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 25 | ConstructorInitializerIndentWidth: 4 26 | DerivePointerAlignment: false 27 | ExperimentalAutoDetectBinPacking: false 28 | IndentCaseLabels: true 29 | IndentWrappedFunctionNames: false 30 | IndentFunctionDeclarationAfterType: false 31 | MaxEmptyLinesToKeep: 1 32 | KeepEmptyLinesAtTheStartOfBlocks: false 33 | NamespaceIndentation: None 34 | ObjCBlockIndentWidth: 2 35 | ObjCSpaceAfterProperty: false 36 | ObjCSpaceBeforeProtocolList: true 37 | PenaltyBreakBeforeFirstCallParameter: 19 38 | PenaltyBreakComment: 300 39 | PenaltyBreakString: 1000 40 | PenaltyBreakFirstLessLess: 120 41 | PenaltyExcessCharacter: 1000000 42 | PenaltyReturnTypeOnItsOwnLine: 60 43 | PointerAlignment: Left 44 | SpacesBeforeTrailingComments: 2 45 | Cpp11BracedListStyle: true 46 | Standard: Cpp11 47 | IndentWidth: 4 48 | TabWidth: 8 49 | UseTab: Never 50 | BreakBeforeBraces: Linux # the position of the braces 51 | SpacesInParentheses: false 52 | SpacesInSquareBrackets: false 53 | SpacesInAngles: false 54 | SpaceInEmptyParentheses: false 55 | SpacesInCStyleCastParentheses: false 56 | SpaceAfterCStyleCast: false 57 | SpacesInContainerLiterals: true 58 | SpaceBeforeAssignmentOperators: true 59 | ContinuationIndentWidth: 4 60 | CommentPragmas: '.*' 61 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 62 | SpaceBeforeParens: ControlStatements 63 | DisableFormat: false 64 | ... 65 | -------------------------------------------------------------------------------- /examples/ExPolynomial1D.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ExPolynomial1D.cpp 3 | * 4 | * @brief test polnomial 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-08-11 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | int main(int argc, char* argv[]) 19 | { 20 | using Polynomial = robotics::maths::PowerBasisPolynomial1D; 21 | 22 | // std::vector coeffs{1, -1, 5, -2}; 23 | // std::vector otherCoeffs{-2, 3}; 24 | 25 | // Polynomial polynomial(coeffs); 26 | // Polynomial otherPolynomial(otherCoeffs); 27 | 28 | // auto sum = polynomial.add(otherPolynomial); 29 | // auto multiplication = polynomial.multiply(otherPolynomial); 30 | 31 | // std::cout << multiplication << "\n"; 32 | // std::cout << multiplication.derivative() << "\n"; 33 | 34 | // Polynomial polynomial0(std::vector{8.5, 3.45, 0.0, 0.0, 5.7, 4.9, 0.1, 10}); 35 | 36 | // Polynomial u(std::vector{8, 10, -5, 3}); 37 | // Polynomial v(std::vector{-3, 2, 1}); 38 | // auto qr = u.divide(v); 39 | 40 | // std::cout << qr.first << "\n"; 41 | // std::cout << qr.second << "\n"; 42 | 43 | // robotics::maths::SturmSequence ss(polynomial0); 44 | 45 | // for (const auto& seq : ss.sturmSeqs()) { 46 | // std::cout << seq << "\n"; 47 | // } 48 | 49 | Polynomial polynomial(std::vector{8, -20, 14, 1, -4, 1}); 50 | 51 | robotics::maths::SturmSequence ss(polynomial); 52 | for (const auto& seq : ss.sturmSeqs()) { 53 | std::cout << seq << "\n"; 54 | } 55 | 56 | auto roots = ss.solveLocalMinium(-10.0, 10.0); 57 | for (const double root : roots) { 58 | std::cout << root << "\n"; 59 | } 60 | 61 | // Polynomial temp(std::vector{-1, 1}); 62 | // Polynomial temp2(std::vector{-2, 1}); 63 | // Polynomial temp3(std::vector{2, 1}); 64 | 65 | // auto a = temp.multiply(temp).multiply(temp).multiply(temp); 66 | // auto b = temp.multiply(temp2).multiply(temp2); 67 | // auto c = a.multiply(b).multiply(temp3); 68 | 69 | // std::cout << c << "\n"; 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /examples/ExUserDefinedPointType.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ExUserDefinedPointType.cpp 3 | * 4 | * @brief test bezier curve with user-defined point type 5 | * 6 | * @author bt 7 | * 8 | * @date 2019-07-20 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | class NewPointType : public Eigen::Vector3d 19 | { 20 | public: 21 | NewPointType() : Eigen::Vector3d() 22 | { 23 | } 24 | 25 | template 26 | explicit NewPointType(const Eigen::MatrixBase& other) : Eigen::Vector3d(other) 27 | { 28 | } 29 | 30 | template NewPointType& operator=(const Eigen::MatrixBase& other) 31 | { 32 | this->Eigen::Vector3d::operator=(other); 33 | return *this; 34 | } 35 | 36 | NewPointType(const Eigen::Vector3d& other, double otherVariable) 37 | : Eigen::Vector3d(other), _otherVariable(otherVariable) 38 | { 39 | } 40 | 41 | explicit NewPointType(double* v) : Eigen::Vector3d(v) 42 | { 43 | } 44 | 45 | private: 46 | double _otherVariable; 47 | }; 48 | 49 | int main(int argc, char* argv[]) 50 | { 51 | using EigenBezier = robotics::Bezier; 52 | EigenBezier::VecPointType eigenPV{{1, 2, 5}, {2, 6, 3}, {3, 2, 3}, {4, 3, 3}, {5, 13, 5}, 53 | {6, 4, 7}, {7, 1, 9}, {8, 2, 11}, {9, 4, 9}, {10, 8, 10}}; 54 | 55 | using Bezier = robotics::Bezier; 56 | Bezier::VecPointType pV; 57 | 58 | std::transform(eigenPV.begin(), eigenPV.end(), std::back_inserter(pV), 59 | [](const EigenBezier::PointType& p) { return Bezier::PointType(p, 10); }); 60 | 61 | Bezier::Ptr bezier = std::make_shared(9, pV); 62 | auto coeffV = bezier->binomialCoeffs(); 63 | 64 | const Bezier::Tangent tan = bezier->tangent(0.7); 65 | const Bezier::Normal nor = bezier->normal(0.7); 66 | double curvature = bezier->curvature(0.7); 67 | 68 | std::cout << "tangent vector: \n" << tan << "\n"; 69 | std::cout << "normal vector: \n" << nor << "\n"; 70 | std::cout << "dot product: " << tan.dot(nor) << "\n"; 71 | std::cout << "curvature: " << curvature << "\n"; 72 | 73 | std::cout << "Original control points: \n"; 74 | for (const auto& p : bezier->controlPoints()) { 75 | std::cout << p[0] << "," << p[1] << "," << p[2] << "\n"; 76 | } 77 | 78 | std::cout << "Trajectory: \n"; 79 | Bezier::VecPointType trajectory = bezier->trajectory(20); 80 | for (const auto& p : trajectory) { 81 | std::cout << p[0] << "," << p[1] << "," << p[2] << "\n"; 82 | } 83 | 84 | std::cout << *bezier << "\n"; 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /examples/ExBoundingBox.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ExBoundingBox.cpp 3 | * 4 | * Copyright (c) organization 5 | * 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #ifndef WITH_VISUALIZATION 14 | #define ENABLE_VISUALIZATION 1 15 | #endif // WITH_VISUALIZATION 16 | 17 | #if ENABLE_VISUALIZATION 18 | #include 19 | #endif // ENABLE_VISUALIZATION 20 | 21 | enum MPL_PATH { STOP = 0, MOVETO = 1, LINETO = 2, CURVE3 = 3, CURVE4 = 4, CLOSEPOLY = 5 }; 22 | 23 | int main(int argc, char* argv[]) 24 | { 25 | using Bezier = robotics::Bezier; 26 | const Bezier::VecPointType pV{{1.1, -0.6}, {3.2, 0.9}, {4.9, 2.4}, {5.9, 3.8}, {7.2, 2.13}}; 27 | 28 | Bezier bezier(4, pV); 29 | 30 | const Bezier::VecPointType trajectory = bezier.trajectory(20); 31 | 32 | const Bezier::BoundingBox bbox = bezier.estimateBoundingBox(); 33 | 34 | #if ENABLE_VISUALIZATION 35 | // import modules of matplotlib library 36 | pe::vis::Matplotlib mpllib; 37 | 38 | // check if the modules are imported successully or not 39 | if (!mpllib.imported()) { 40 | std::cout << "Failed to import matplotlib library\n"; 41 | exit(EXIT_FAILURE); 42 | } 43 | 44 | const auto xyS = bezier.extractDataEachAxis(pV); 45 | const auto xyTrajectoryS = bezier.extractDataEachAxis(trajectory); 46 | 47 | mpllib.axes(5, 5); 48 | std::vector> vertices = { 49 | {bbox[0].x(), bbox[0].y()}, {bbox[1].x(), bbox[0].y()}, {bbox[1].x(), bbox[1].y()}, 50 | {bbox[0].x(), bbox[1].y()}, {bbox[0].x(), bbox[0].y()}, 51 | }; 52 | 53 | std::vector codes = {MOVETO, LINETO, LINETO, LINETO, CLOSEPOLY}; 54 | 55 | mpllib.add_patch(vertices, codes, 56 | { 57 | {"facecolor", pe::vis::Matplotlib::createAnyBaseMapData("None")}, 58 | {"edgecolor", pe::vis::Matplotlib::createAnyBaseMapData("green")}, 59 | {"lw", pe::vis::Matplotlib::createAnyBaseMapData(2)}, 60 | }); 61 | 62 | mpllib.plot(xyS[0], xyS[1], 63 | { 64 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("control points")}, 65 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("g")}, 66 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 67 | }); 68 | 69 | mpllib.scatter(xyS[0], xyS[1]); 70 | 71 | mpllib.plot(xyTrajectoryS[0], xyTrajectoryS[1], 72 | { 73 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("approximated bezier curve")}, 74 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("r")}, 75 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 76 | }); 77 | 78 | mpllib.legend(); 79 | 80 | mpllib.grid(); 81 | 82 | mpllib.savefig("boundingbox.png"); 83 | 84 | mpllib.show(); 85 | 86 | #endif // ENABLE_VISUALIZATION 87 | return EXIT_SUCCESS; 88 | } 89 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/xmba15/bezier_curve.svg?branch=master)](https://travis-ci.org/xmba15/bezier_curve/builds) 2 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 3 | 4 | # Generic Bezier Curve Library # 5 | - Generic Bezier Curve Library that supports control points of arbitary dimension numbers. (2D and 3D for normal, curvature calculations). 6 | - Class of control point Container is built on top of Eigen Matrix; but can possibly be replaced with user-defined Point Container Class (which is quite a tedious task if starting from 0). 7 | 8 | ## Dependencies ## 9 | - [Eigen](http://eigen.tuxfamily.org) : **Current build with cmake was tested on Ubuntu 18.04. Build on lower version of Ubuntu OS might require modification of CMake to find Eigen OR manually build latest Eigen from source.** 10 | - [Doxygen](http://www.doxygen.nl/index.html) : Only used for building documentations. The lack of doxygen does not affect the building of this library. 11 | 12 | ## How to Build ## 13 | ```bash 14 | cd /path/to/bezier_curve 15 | mkdir build && cd build 16 | cmake ../ && cmake --build . 17 | ``` 18 | 19 | ### Build Options ### 20 | - WITH_DEBUG=ON/OFF(default: OFF) : whether to enable debug 21 | - WITH_GTEST=ON/OFF(default: OFF) : whether to enable gtest 22 | - BUILD_EXAMPLES=ON/OFF(default: OFF) : whether to enable build examples 23 | - BUILD_DOC=ON/OFF(default: ON) : whether to build doc (by doxygen) or not 24 | - WITH_VISUALIZATION=ON/OFF(default: OFF) : whether to build examples with visualization (currently used [matplotlib_cpp](https://github.com/xmba15/Another_MatplotlibCpp.git) for visualization) 25 | 26 | ## General Usage ## 27 | See [Examples](examples) 28 | 29 | ## Examples ## 30 | ### 2d points ## 31 | 32 | Example for 5-degree bezier curve of the following 2d control points: 33 | 34 | [Source Code](./examples/Ex2DPoints.cpp) 35 | 36 |

37 | 38 |

39 | 40 | ### 3d points ## 41 | 42 | Example for 9-degree bezier curve of the following 3d control points: 43 | 44 | [Source Code](./examples/Ex3DPoints.cpp) 45 | 46 |

47 | 48 |

49 | 50 | ### point projection from an external point to bezier curve ### 51 | 52 | In this function, I implemented the algorithm presented in the following paper: 53 | 54 | ``` 55 | Xiao-Diao Chen, Yin Zhou, Zhenyu Shu, Hua Su, Jean-Claude Paul. Improved Algebraic Algorithm 56 | On Point Projection For Bézier Curves. Proceedings of the Second International Multi-Symposiums 57 | on Computer and Computational Sciences (IMSCCS 2007), The University of Iowa, Iowa City, Iowa, 58 | USA, Aug 2007, Iowa, United States. pp.158-163, ff10.1109/IMSCCS.2007.17ff. ffinria-00518379f 59 | ``` 60 | 61 | [Source Code](./examples/ClosestPointsToCurve.cpp) 62 | 63 |

64 | 65 |

66 | 67 | ### bounding box for bezier curve ### 68 | 69 | [Source Code](./examples/ExBoundingBox.cpp) 70 | 71 |

72 | 73 |

74 | 75 | 76 | ## Acknowledgement ## 77 | - This library is inspired by the following [Bezier library](https://github.com/oysteinmyrmo/bezier), which supports only 2D space (as of 21st June, 2019). 78 | - I partly reused the code introduced in ```https://computingandrecording.wordpress.com/2017/03/20/closest-point-to-a-cubic-spline/``` for the functionality of point projection. 79 | -------------------------------------------------------------------------------- /examples/Ex2DPoints.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Ex1.cpp 3 | * 4 | * @brief test basic function of Bezier Class 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-19 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #ifndef WITH_VISUALIZATION 21 | #define ENABLE_VISUALIZATION 1 22 | #endif // WITH_VISUALIZATION 23 | 24 | #if ENABLE_VISUALIZATION 25 | #include 26 | #endif // ENABLE_VISUALIZATION 27 | 28 | int main(int argc, char* argv[]) 29 | { 30 | using Bezier = robotics::Bezier; 31 | Bezier::PointType p1(10.83, 6.44); 32 | Bezier::PointType p2(27.99, 9.75); 33 | Bezier::PointType p3(43.91, 14.3); 34 | Bezier::PointType p4(67.48, 24.84); 35 | Bezier::PointType p5(75.34, 46.97); 36 | Bezier::PointType p6(65.33, 86.25); 37 | 38 | Bezier::VecPointType pV{p1, p2, p3, p4, p5, p6}; 39 | 40 | Bezier::Ptr bezier = std::make_shared(5, pV); 41 | auto coeffV = bezier->binomialCoeffs(); 42 | 43 | const Bezier::Tangent tan = bezier->tangent(0.7); 44 | const Bezier::Normal nor = bezier->normal(0.7); 45 | double curvature = bezier->curvature(0.7); 46 | 47 | std::cout << "tangent vector: \n" << tan << "\n"; 48 | std::cout << "normal vector: \n" << nor << "\n"; 49 | std::cout << "dot product: " << tan.dot(nor) << "\n"; 50 | std::cout << "curvature: " << curvature << "\n"; 51 | 52 | std::cout << "Original control points: \n"; 53 | for (const auto& p : bezier->controlPoints()) { 54 | std::cout << p[0] << "," << p[1] << "\n"; 55 | } 56 | 57 | std::cout << "Trajectory: \n"; 58 | Bezier::VecPointType trajectory = bezier->trajectory(20); 59 | for (const auto& p : trajectory) { 60 | std::cout << p[0] << "," << p[1] << "\n"; 61 | } 62 | 63 | std::cout << *bezier << "\n"; 64 | 65 | #if ENABLE_VISUALIZATION 66 | // import modules of matplotlib library 67 | pe::vis::Matplotlib mpllib; 68 | 69 | // check if the modules are imported successully or not 70 | if (!mpllib.imported()) { 71 | std::cout << "Failed to import matplotlib library\n"; 72 | exit(EXIT_FAILURE); 73 | } 74 | 75 | const auto xyS = bezier->extractDataEachAxis(pV); 76 | const auto xyTrajectoryS = bezier->extractDataEachAxis(trajectory); 77 | 78 | mpllib.plot(xyS[0], xyS[1], 79 | { 80 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("control points")}, 81 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("g")}, 82 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 83 | }); 84 | 85 | mpllib.scatter(xyS[0], xyS[1]); 86 | 87 | mpllib.plot(xyTrajectoryS[0], xyTrajectoryS[1], 88 | { 89 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("approximated bezier curve")}, 90 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("r")}, 91 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 92 | }); 93 | 94 | mpllib.legend(); 95 | 96 | mpllib.grid(); 97 | 98 | mpllib.savefig("2dpoints.png"); 99 | 100 | mpllib.show(); 101 | 102 | #endif // ENABLE_VISUALIZATION 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /examples/Ex3DPoints.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Ex1.cpp 3 | * 4 | * @brief test basic function of Bezier Class 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-19 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #ifndef WITH_VISUALIZATION 20 | #define ENABLE_VISUALIZATION 1 21 | #endif // WITH_VISUALIZATION 22 | 23 | #if ENABLE_VISUALIZATION 24 | #include 25 | #endif // ENABLE_VISUALIZATION 26 | 27 | int main(int argc, char* argv[]) 28 | { 29 | using Bezier = robotics::Bezier; 30 | Bezier::PointType p1(1, 5, 2); 31 | Bezier::PointType p2(2, 6, 3); 32 | Bezier::PointType p3(3, 2, 3); 33 | Bezier::PointType p4(4, 3, 3); 34 | Bezier::PointType p5(5, 13, 5); 35 | Bezier::PointType p6(6, 4, 7); 36 | Bezier::PointType p7(7, 1, 9); 37 | Bezier::PointType p8(8, 2, 11); 38 | Bezier::PointType p9(9, 4, 9); 39 | Bezier::PointType p10(10, 8, 10); 40 | 41 | Bezier::VecPointType pV{p1, p2, p3, p4, p5, p6, p7, p8, p9, p10}; 42 | 43 | Bezier::Ptr bezier = std::make_shared(9, pV); 44 | auto coeffV = bezier->binomialCoeffs(); 45 | 46 | const Bezier::Tangent tan = bezier->tangent(0.7); 47 | const Bezier::Normal nor = bezier->normal(0.7); 48 | double curvature = bezier->curvature(0.7); 49 | 50 | std::cout << "tangent vector: \n" << tan << "\n"; 51 | std::cout << "normal vector: \n" << nor << "\n"; 52 | std::cout << "dot product: " << tan.dot(nor) << "\n"; 53 | std::cout << "curvature: " << curvature << "\n"; 54 | 55 | std::cout << "Original control points: \n"; 56 | for (const auto& p : bezier->controlPoints()) { 57 | std::cout << p[0] << "," << p[1] << "," << p[2] << "\n"; 58 | } 59 | 60 | std::cout << "Trajectory: \n"; 61 | Bezier::VecPointType trajectory = bezier->trajectory(20); 62 | for (const auto& p : trajectory) { 63 | std::cout << p[0] << "," << p[1] << "," << p[2] << "\n"; 64 | } 65 | 66 | std::cout << *bezier << "\n"; 67 | 68 | #if ENABLE_VISUALIZATION 69 | // import modules of matplotlib library 70 | pe::vis::Matplotlib mpllib; 71 | 72 | // check if the modules are imported successully or not 73 | if (!mpllib.imported()) { 74 | std::cout << "Failed to import matplotlib library\n"; 75 | exit(EXIT_FAILURE); 76 | } 77 | 78 | const auto xyzS = bezier->extractDataEachAxis(pV); 79 | 80 | mpllib.initializeAxes3D(); 81 | 82 | mpllib.plotAxes3D( 83 | xyzS[0], xyzS[1], xyzS[2], 84 | { 85 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("control points")}, 86 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("g")}, 87 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 88 | }); 89 | mpllib.scatterAxes3D(xyzS[0], xyzS[1], xyzS[2]); 90 | 91 | const auto xyzTrajectoryS = bezier->extractDataEachAxis(trajectory); 92 | 93 | mpllib.plotAxes3D( 94 | xyzTrajectoryS[0], xyzTrajectoryS[1], xyzTrajectoryS[2], 95 | { 96 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("approximated bezier curve")}, 97 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("r")}, 98 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 99 | }); 100 | 101 | mpllib.legend(); 102 | 103 | mpllib.savefig("3dpoints.png"); 104 | 105 | mpllib.show(); 106 | #endif // ENABLE_VISUALIZATION 107 | 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /examples/ClosestPointsToCurve.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ClosestPointsToCurve.cpp 3 | * 4 | * @author btran 5 | * 6 | * @date 2019-08-17 7 | * 8 | * Copyright (c) organization 9 | * 10 | */ 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #ifndef WITH_VISUALIZATION 18 | #define ENABLE_VISUALIZATION 1 19 | #endif // WITH_VISUALIZATION 20 | 21 | #if ENABLE_VISUALIZATION 22 | #include 23 | #endif // ENABLE_VISUALIZATION 24 | 25 | int main(int argc, char* argv[]) 26 | { 27 | using Bezier = robotics::Bezier; 28 | const Bezier::VecPointType pV{{1.0, 0.0}, {3.0, 0.0}, {4.9, 2.4}, {5.9, 3.0}, {7.2, 3.0}}; 29 | 30 | Bezier bezier(4, pV); 31 | 32 | const Bezier::VecPointType trajectory = bezier.trajectory(20); 33 | 34 | const Bezier::VecPointType outliners{{0.5, 0}, {2, 0.5}, {2.5, 0.1}, {2.5, 1.5}, {3.0, 0.0}, 35 | {3.5, 1.9}, {4.8, 0.6}, {5.2, 1.25}, {6, 1.7}}; 36 | Bezier::VecPointType closestPoints; 37 | closestPoints.reserve(outliners.size()); 38 | 39 | for (const auto& outliner : outliners) { 40 | const double closestVal = bezier.closestPointToCurve(outliner); 41 | closestPoints.emplace_back(bezier(closestVal)); 42 | } 43 | 44 | #if ENABLE_VISUALIZATION 45 | // import modules of matplotlib library 46 | pe::vis::Matplotlib mpllib; 47 | 48 | // check if the modules are imported successully or not 49 | if (!mpllib.imported()) { 50 | std::cout << "Failed to import matplotlib library\n"; 51 | exit(EXIT_FAILURE); 52 | } 53 | 54 | const auto xyS = bezier.extractDataEachAxis(pV); 55 | const auto xyTrajectoryS = bezier.extractDataEachAxis(trajectory); 56 | 57 | mpllib.plot(xyS[0], xyS[1], 58 | { 59 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("control points")}, 60 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("g")}, 61 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 62 | }); 63 | 64 | mpllib.scatter(xyS[0], xyS[1]); 65 | 66 | mpllib.plot(xyTrajectoryS[0], xyTrajectoryS[1], 67 | { 68 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("approximated bezier curve")}, 69 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("r")}, 70 | {"linestyle", pe::vis::Matplotlib::createAnyBaseMapData("--")}, 71 | }); 72 | 73 | for (size_t i = 0; i < outliners.size(); ++i) { 74 | const auto outliner = outliners[i]; 75 | const auto closestPoint = closestPoints[i]; 76 | 77 | mpllib.plot(std::vector{outliner.x(), closestPoint.x()}, 78 | std::vector{outliner.y(), closestPoint.y()}, 79 | { 80 | {"label", pe::vis::Matplotlib::createAnyBaseMapData("outliner to closest point")}, 81 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("y")}, 82 | }); 83 | 84 | mpllib.scatter(std::vector{outliner.x(), closestPoint.x()}, 85 | std::vector{outliner.y(), closestPoint.y()}, 86 | { 87 | {"color", pe::vis::Matplotlib::createAnyBaseMapData("b")}, 88 | }); 89 | } 90 | 91 | mpllib.legend(); 92 | 93 | mpllib.grid(); 94 | 95 | mpllib.savefig("pointprojection.png"); 96 | 97 | mpllib.show(); 98 | 99 | #endif // ENABLE_VISUALIZATION 100 | return EXIT_SUCCESS; 101 | } 102 | -------------------------------------------------------------------------------- /tests/TestBezier.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file TestBezier.cpp 3 | * 4 | * @brief Test Basic Function of Bezier 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-20 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #include "gtest/gtest.h" 15 | #include 16 | #include 17 | 18 | class TestBezier : public ::testing::Test 19 | { 20 | protected: 21 | void SetUp() override 22 | { 23 | start_time_ = time(nullptr); 24 | } 25 | 26 | void TearDown() override 27 | { 28 | const time_t end_time = time(nullptr); 29 | 30 | // expect test time less than 5 sec 31 | EXPECT_LE(end_time - start_time_, 5); 32 | } 33 | 34 | time_t start_time_; 35 | }; 36 | 37 | TEST_F(TestBezier, TestInitialization) 38 | { 39 | using Bezier5d = robotics::Bezier; 40 | using Bezier10d = robotics::Bezier; 41 | using Bezier15d = robotics::Bezier; 42 | using Bezier20d = robotics::Bezier; 43 | using Bezier100d = robotics::Bezier; 44 | 45 | { 46 | ASSERT_EQ(Bezier5d(5).binomialCoeffs().size(), 6); 47 | ASSERT_EQ(Bezier100d(100).binomialCoeffs().size(), 101); 48 | } 49 | 50 | { 51 | const std::vector binomialCoeffs = Bezier5d(5).binomialCoeffs(); 52 | std::vector expectedBinomialCoeffs{1, 5, 10, 10, 5, 1}; 53 | 54 | EXPECT_EQ(binomialCoeffs, expectedBinomialCoeffs); 55 | } 56 | 57 | { 58 | const std::vector binomialCoeffs = Bezier10d(10).binomialCoeffs(); 59 | std::vector expectedBinomialCoeffs{1, 10, 45, 120, 210, 252, 210, 120, 45, 10, 1}; 60 | 61 | EXPECT_EQ(binomialCoeffs, expectedBinomialCoeffs); 62 | } 63 | 64 | { 65 | const std::vector binomialCoeffs = Bezier15d(15).binomialCoeffs(); 66 | std::vector expectedBinomialCoeffs{1, 15, 105, 455, 1365, 3003, 5005, 6435, 67 | 6435, 5005, 3003, 1365, 455, 105, 15, 1}; 68 | 69 | EXPECT_EQ(binomialCoeffs, expectedBinomialCoeffs); 70 | } 71 | 72 | { 73 | const std::vector binomialCoeffs = Bezier20d(20).binomialCoeffs(); 74 | std::vector expectedBinomialCoeffs{1, 20, 190, 1140, 4845, 15504, 38760, 75 | 77520, 125970, 167960, 184756, 167960, 125970, 77520, 76 | 38760, 15504, 4845, 1140, 190, 20, 1}; 77 | 78 | EXPECT_EQ(binomialCoeffs, expectedBinomialCoeffs); 79 | } 80 | } 81 | 82 | TEST_F(TestBezier, TestControlPoints) 83 | { 84 | using Bezier3d = robotics::Bezier; 85 | Bezier3d::VecPointType v({{120, 160}, {35, 200}, {220, 260}, {220, 40}}); 86 | Bezier3d bezier3d(3, v); 87 | double t; 88 | 89 | { 90 | t = 0; 91 | Bezier3d::PointType p = bezier3d(t); 92 | Bezier3d::PointType expectedP; 93 | expectedP << 120 + Bezier3d::TOLERANCE, 160; 94 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP, Bezier3d::TOLERANCE)); 95 | } 96 | 97 | { 98 | t = 1; 99 | Bezier3d::PointType p = bezier3d(t); 100 | Bezier3d::PointType expectedP; 101 | expectedP << 220 + 0.00001, 160 + 0.001; 102 | EXPECT_FALSE(Bezier3d::fuzzyEquals(p, expectedP)); 103 | } 104 | 105 | { 106 | t = 0.25; 107 | Bezier3d::PointType p = bezier3d(t); 108 | Bezier3d::PointType expectedP; 109 | expectedP << 99.765625, 189.0625; 110 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP)); 111 | } 112 | 113 | { 114 | t = 0.5; 115 | Bezier3d::PointType p = bezier3d(t); 116 | Bezier3d::PointType expectedP; 117 | expectedP << 138.125, 197.5; 118 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP)); 119 | } 120 | 121 | { 122 | t = 0.75; 123 | Bezier3d::PointType p = bezier3d(t); 124 | Bezier3d::PointType expectedP; 125 | expectedP << 192.421875, 157.1875; 126 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP)); 127 | } 128 | 129 | { 130 | t = -0.35; 131 | Bezier3d::PointType p = bezier3d(t); 132 | Bezier3d::PointType expectedP; 133 | expectedP << p.x() + std::numeric_limits::epsilon(), p.y() + std::numeric_limits::epsilon(); 134 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP)); 135 | } 136 | 137 | { 138 | t = 1.5; 139 | Bezier3d::PointType p = bezier3d(t); 140 | Bezier3d::PointType expectedP; 141 | expectedP << 24.375, -537.5; 142 | EXPECT_TRUE(Bezier3d::fuzzyEquals(p, expectedP)); 143 | } 144 | } 145 | -------------------------------------------------------------------------------- /include/bezier/PowerBasisPolynomial1D.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PowerBasisPolyNomial1D.hpp 3 | * 4 | * @author btran 5 | * 6 | * @date 2019-08-11 7 | * 8 | * Copyright (c) organization 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "bezier/NumericalMaths.hpp" 23 | 24 | namespace robotics 25 | { 26 | namespace maths 27 | { 28 | template class PowerBasisPolynomial1D 29 | { 30 | public: 31 | static constexpr DATA_TYPE TOLERANCE = 10e-7; 32 | 33 | using QuotientRemainder = std::pair; 34 | 35 | explicit PowerBasisPolynomial1D(const std::vector& coeffs, const bool normalized = false, 36 | const DATA_TYPE tolerance = TOLERANCE); 37 | 38 | PowerBasisPolynomial1D(const PowerBasisPolynomial1D& other, const bool normalized = false); 39 | 40 | DATA_TYPE operator()(const DATA_TYPE t) const; 41 | 42 | const std::vector& coeffs() const; 43 | 44 | const size_t degree() const; 45 | 46 | bool isZero() const; 47 | 48 | PowerBasisPolynomial1D add(const PowerBasisPolynomial1D& other) const; 49 | 50 | PowerBasisPolynomial1D& operator+=(const PowerBasisPolynomial1D& other); 51 | 52 | PowerBasisPolynomial1D multiply(const PowerBasisPolynomial1D& other) const; 53 | 54 | PowerBasisPolynomial1D multiply(const DATA_TYPE scalar) const; 55 | 56 | PowerBasisPolynomial1D derivative() const; 57 | 58 | QuotientRemainder divide(const PowerBasisPolynomial1D& other) const; 59 | 60 | PowerBasisPolynomial1D operator%(const PowerBasisPolynomial1D& other) const; 61 | 62 | static PowerBasisPolynomial1D gcd(const PowerBasisPolynomial1D& firstPoly, 63 | const PowerBasisPolynomial1D& secondPoly); 64 | 65 | bool isMonic() const; 66 | 67 | PowerBasisPolynomial1D monicized() const; 68 | 69 | friend std::ostream& operator<<(std::ostream& os, const PowerBasisPolynomial1D& polynomial) 70 | { 71 | assert(polynomial.degree() >= 0); 72 | 73 | os << "Polynomial function: \n"; 74 | for (size_t i = 0; i < polynomial.coeffs().size(); ++i) { 75 | if (combinedToleranceEquals(polynomial.coeffs()[i], static_cast(0), polynomial._tolerance)) { 76 | continue; 77 | } 78 | 79 | #if ENABLE_DEBUG 80 | os << " + " << std::setw(8) << polynomial.coeffs()[i] << " * std::pow(t," << i << ")"; 81 | #else 82 | os << " + " << std::setw(8) << polynomial.coeffs()[i] << " * t^" << i; 83 | #endif // ENABLE_DEBUG 84 | } 85 | 86 | return os; 87 | } 88 | 89 | private: 90 | void removeLeadingZeros(); 91 | void monicize(); 92 | 93 | private: 94 | std::vector _coeffs; 95 | DATA_TYPE _tolerance; 96 | }; 97 | 98 | template 99 | PowerBasisPolynomial1D::PowerBasisPolynomial1D(const std::vector& coeffs, const bool normalized, 100 | const DATA_TYPE tolerance) 101 | : _coeffs(coeffs), _tolerance(tolerance) 102 | { 103 | assert(this->_coeffs.size() > 0); 104 | this->removeLeadingZeros(); 105 | 106 | if (normalized && !this->isMonic()) { 107 | this->monicize(); 108 | } 109 | } 110 | 111 | template 112 | PowerBasisPolynomial1D::PowerBasisPolynomial1D(const PowerBasisPolynomial1D& other, 113 | const bool normalized) 114 | { 115 | this->_coeffs = other._coeffs; 116 | this->_tolerance = other._tolerance; 117 | 118 | if (normalized && !this->isMonic()) { 119 | this->monicize(); 120 | } 121 | } 122 | 123 | template DATA_TYPE PowerBasisPolynomial1D::operator()(const DATA_TYPE t) const 124 | { 125 | DATA_TYPE result = static_cast(0); 126 | 127 | for (size_t i = 0; i < this->_coeffs.size(); ++i) { 128 | result += this->_coeffs[i] * std::pow(t, i); 129 | } 130 | 131 | return result; 132 | } 133 | 134 | template const std::vector& PowerBasisPolynomial1D::coeffs() const 135 | { 136 | return this->_coeffs; 137 | } 138 | 139 | template const size_t PowerBasisPolynomial1D::degree() const 140 | { 141 | return this->_coeffs.size() - 1; 142 | } 143 | 144 | template bool PowerBasisPolynomial1D::isZero() const 145 | { 146 | return (this->degree() == 0 && 147 | combinedToleranceEquals(this->_coeffs.back(), static_cast(0), this->_tolerance)); 148 | } 149 | 150 | template 151 | PowerBasisPolynomial1D 152 | PowerBasisPolynomial1D::add(const PowerBasisPolynomial1D& other) const 153 | { 154 | std::vector biggerDegCoeffs = 155 | this->_coeffs.size() >= other._coeffs.size() ? this->_coeffs : other._coeffs; 156 | std::vector smallerDegCoeffs = 157 | this->_coeffs.size() < other._coeffs.size() ? this->_coeffs : other._coeffs; 158 | 159 | for (size_t i = 0; i < smallerDegCoeffs.size(); ++i) { 160 | biggerDegCoeffs[i] += smallerDegCoeffs[i]; 161 | } 162 | 163 | return PowerBasisPolynomial1D(biggerDegCoeffs); 164 | } 165 | 166 | template 167 | PowerBasisPolynomial1D& PowerBasisPolynomial1D::operator+=(const PowerBasisPolynomial1D& other) 168 | { 169 | *this = this->add(other); 170 | return *this; 171 | } 172 | 173 | template 174 | PowerBasisPolynomial1D addPolynomials(const PowerBasisPolynomial1D& p1, 175 | const PowerBasisPolynomial1D& p2) 176 | { 177 | return p1.add(p2); 178 | } 179 | 180 | template 181 | PowerBasisPolynomial1D 182 | PowerBasisPolynomial1D::multiply(const PowerBasisPolynomial1D& other) const 183 | { 184 | // naive approach for now 185 | // better approach using fft 186 | // [source](http://people.csail.mit.edu/madhu/ST12/scribe/lect06.pdf) 187 | std::vector resultCoeffs(this->_coeffs.size() + other._coeffs.size(), 0.0); 188 | 189 | for (size_t i = 0; i < this->_coeffs.size(); ++i) { 190 | for (size_t io = 0; io < other._coeffs.size(); ++io) { 191 | resultCoeffs[i + io] += this->_coeffs[i] * other._coeffs[io]; 192 | } 193 | } 194 | 195 | return PowerBasisPolynomial1D(resultCoeffs); 196 | } 197 | 198 | template 199 | PowerBasisPolynomial1D PowerBasisPolynomial1D::multiply(const DATA_TYPE scalar) const 200 | { 201 | std::vector resultCoeffs; 202 | resultCoeffs.reserve(this->_coeffs.size()); 203 | 204 | std::transform(this->_coeffs.begin(), this->_coeffs.end(), std::back_inserter(resultCoeffs), 205 | [&scalar](const DATA_TYPE val) { return val * scalar; }); 206 | 207 | return PowerBasisPolynomial1D(resultCoeffs); 208 | } 209 | 210 | template 211 | PowerBasisPolynomial1D multiplyPolynomials(const PowerBasisPolynomial1D& p1, 212 | const PowerBasisPolynomial1D& p2) 213 | { 214 | return p1.multiply(p2); 215 | } 216 | 217 | template PowerBasisPolynomial1D PowerBasisPolynomial1D::derivative() const 218 | { 219 | assert(this->_coeffs.size() > 0); 220 | if (this->_coeffs.size() == 1) { 221 | return PowerBasisPolynomial1D(std::vector{0}); 222 | } 223 | 224 | std::vector derivCoeffs(this->_coeffs.begin() + 1, this->_coeffs.end()); 225 | for (size_t i = 0; i < derivCoeffs.size(); ++i) { 226 | derivCoeffs[i] *= (i + 1); 227 | } 228 | 229 | return PowerBasisPolynomial1D(derivCoeffs); 230 | } 231 | 232 | template 233 | typename PowerBasisPolynomial1D::QuotientRemainder 234 | PowerBasisPolynomial1D::divide(const PowerBasisPolynomial1D& other) const 235 | { 236 | // naive approach for now 237 | // [source](http://web.cs.iastate.edu/~cs577/handouts/polydivide.pdf) 238 | if (other.degree() == 0) { 239 | if (combinedToleranceEquals(other._coeffs.front(), static_cast(0), this->_tolerance)) { 240 | throw std::runtime_error("cannot divide by 0"); 241 | } else { 242 | return std::make_pair(this->multiply(1.0 / other._coeffs.front()), 243 | PowerBasisPolynomial1D(std::vector{0})); 244 | } 245 | } 246 | 247 | if (this->degree() < other.degree()) { 248 | return std::make_pair(PowerBasisPolynomial1D(std::vector{0}), *this); 249 | } 250 | 251 | // N / D == q; N % D == r 252 | std::vector N = this->_coeffs; 253 | std::vector D = other._coeffs; 254 | std::vector q(this->degree() - other.degree() + 1, 0); 255 | std::vector r; 256 | r.reserve(other.degree()); 257 | 258 | for (int k = (this->degree() - other.degree()); k >= 0; --k) { 259 | q[k] = N[other.degree() + k] / D.back(); 260 | if (combinedToleranceEquals(q[k], static_cast(0), this->_tolerance)) { 261 | q[k] = static_cast(0); 262 | } 263 | 264 | for (int j = other.degree() + k - 1; j >= k; --j) { 265 | N[j] -= q[k] * D[j - k]; 266 | } 267 | } 268 | 269 | std::copy(N.begin(), N.begin() + other.degree(), std::back_inserter(r)); 270 | 271 | return std::make_pair(PowerBasisPolynomial1D(q), PowerBasisPolynomial1D(r)); 272 | } 273 | 274 | template 275 | PowerBasisPolynomial1D PowerBasisPolynomial1D:: 276 | operator%(const PowerBasisPolynomial1D& other) const 277 | { 278 | return this->divide(other).second; 279 | } 280 | 281 | template 282 | PowerBasisPolynomial1D 283 | PowerBasisPolynomial1D::gcd(const PowerBasisPolynomial1D& firstPoly, 284 | const PowerBasisPolynomial1D& secondPoly) 285 | { 286 | if (firstPoly.degree() < secondPoly.degree()) { 287 | return gcd(secondPoly, firstPoly); 288 | } 289 | 290 | if (secondPoly.isZero()) { 291 | return PowerBasisPolynomial1D(firstPoly, true); 292 | } 293 | 294 | PowerBasisPolynomial1D remainder = firstPoly % secondPoly; 295 | 296 | return gcd(secondPoly, remainder); 297 | } 298 | 299 | template bool PowerBasisPolynomial1D::isMonic() const 300 | { 301 | return combinedToleranceEquals(std::fabs(this->_coeffs.back()), static_cast(1), this->_tolerance); 302 | } 303 | 304 | template PowerBasisPolynomial1D PowerBasisPolynomial1D::monicized() const 305 | { 306 | if (this->isMonic()) { 307 | return *this; 308 | } 309 | 310 | return PowerBasisPolynomial1D(*this, true); 311 | } 312 | 313 | template void PowerBasisPolynomial1D::monicize() 314 | { 315 | if (this->isZero()) { 316 | return; 317 | } 318 | 319 | const DATA_TYPE biggestDegCoeffAbs = std::fabs(this->_coeffs.back()); 320 | 321 | for (auto it = this->_coeffs.begin(); it != this->_coeffs.end(); ++it) { 322 | *it /= biggestDegCoeffAbs; 323 | } 324 | } 325 | 326 | template void PowerBasisPolynomial1D::removeLeadingZeros() 327 | { 328 | while (combinedToleranceEquals(this->_coeffs.back(), static_cast(0), this->_tolerance) && 329 | this->_coeffs.size() > 1) { 330 | this->_coeffs.pop_back(); 331 | } 332 | if (combinedToleranceEquals(this->_coeffs.back(), static_cast(0), this->_tolerance)) { 333 | this->_coeffs.back() = static_cast(0); 334 | } 335 | } 336 | 337 | } // namespace maths 338 | } // namespace robotics 339 | -------------------------------------------------------------------------------- /include/bezier/SturmSequence.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SturmSequence.hpp 3 | * 4 | * @author btran 5 | * 6 | * @date 2019-08-12 7 | * 8 | * Copyright (c) organization 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "bezier/PowerBasisPolynomial1D.hpp" 19 | 20 | namespace robotics 21 | { 22 | namespace maths 23 | { 24 | /** 25 | * @brief Sturm Sequence is a sequences of polynomial generated for root separation of a target polynomial 26 | * 27 | * Given the input power basis polynomial Q(t) 28 | * The following sturm sequence (P_i(t))will be generated: 29 | * P_0(t) = Q(t); 30 | * P_1(t) = Q'(t); 31 | * P_n(t) = - P_(n-1)(t) % P_(n-2)(t) 32 | */ 33 | template class SturmSequence 34 | { 35 | public: 36 | //! tolerant floating point precision 37 | static constexpr DATA_TYPE TOLERANCE = 10e-7; 38 | 39 | /** 40 | * @brief constructing sturm sequence with the original polynomial Q(t) 41 | * 42 | * Q(t) will be divided by gcd of (Q(t), Q'(t)) to obtain a square-free polynomial 43 | * the targeted sequences will be generated in this constructor 44 | * 45 | */ 46 | explicit SturmSequence(const PowerBasisPolynomial1D& polynomial, const DATA_TYPE tolerance = TOLERANCE); 47 | 48 | /** 49 | * @brief constant getter of the generated sturm sequences 50 | * 51 | */ 52 | const std::vector>& sturmSeqs() const; 53 | 54 | /** 55 | * @brief count number of sign changes at value t 56 | * 57 | */ 58 | int countSturmSignChanges(DATA_TYPE t) const; 59 | 60 | /** 61 | * @brief solver to separate and find roots at this interval 62 | * 63 | * @return the vector of found roots 64 | */ 65 | std::vector solve(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const; 66 | 67 | /** 68 | * @brief solver to separate and find minima roots at this interval 69 | * 70 | * @return the vector of found roots 71 | */ 72 | std::vector solveLocalMinium(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const; 73 | 74 | /** 75 | * @brief solver to find root at this interval 76 | * 77 | * @return the root at this interval, NAN if none found 78 | */ 79 | DATA_TYPE solveBisection(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const; 80 | 81 | /** 82 | * @brief solver to find root, that would be local minium, at this interval 83 | * 84 | * @return the root at this interval, NAN if none found 85 | */ 86 | DATA_TYPE solveBisectionLocalMinium(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const; 87 | 88 | private: 89 | /** 90 | * @brief the utility struct to store an interval [lowerBound, upperBound] 91 | * 92 | */ 93 | struct Interval { 94 | DATA_TYPE lowerBound; 95 | DATA_TYPE upperBound; 96 | }; 97 | 98 | /** 99 | * @brief utility struct to store an interval with id for identification 100 | */ 101 | struct SturmInterval : public Interval { 102 | SturmInterval(const DATA_TYPE lowerBoundVal, const DATA_TYPE upperBoundVal, const int lowerBoundSign, 103 | const int upperBoundSign, const int id, const int expectedRoots) 104 | : lowerBoundSign(lowerBoundSign), upperBoundSign(upperBoundSign), id(id), expectedRoots(expectedRoots) 105 | { 106 | if (lowerBoundVal > upperBoundVal) { 107 | throw std::runtime_error("lower bound must be less than upper bound"); 108 | } 109 | this->lowerBound = lowerBoundVal; 110 | this->upperBound = upperBoundVal; 111 | } 112 | 113 | //! number of sign changes at lower bound 114 | int lowerBoundSign; 115 | 116 | //! number of sign changes at upper bound 117 | int upperBoundSign; 118 | 119 | //! id of the interval 120 | int id; 121 | 122 | //! expected number of roots held in this interval 123 | int expectedRoots; 124 | }; 125 | 126 | /** 127 | * @brief generate Sturm sequences 128 | * 129 | * @return the generated Sturm sequences 130 | */ 131 | std::vector> generateSturmSequences() const; 132 | 133 | /** 134 | * @brief utility function to separate roots into interval 135 | * 136 | */ 137 | std::pair, int> separateRoots(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const; 138 | 139 | /** 140 | * @brief utility function to find roots using provided solver 141 | * 142 | */ 143 | std::vector solverUtil(const DATA_TYPE lowerBound, const DATA_TYPE upperBound, 144 | const std::function& solver) const; 145 | 146 | private: 147 | //! the original polynomial. Note that the original polynomial will be transformed into a square-free polynomial. 148 | PowerBasisPolynomial1D _polynomial; 149 | 150 | //! member variable to hold the generated Sturm sequences 151 | std::vector> _sturmSeqs; 152 | 153 | //! max division estimated from the tolerant precision 154 | int _maxDivisions; 155 | 156 | //! tolerant floating point precision 157 | DATA_TYPE _tolerance; 158 | }; 159 | 160 | template 161 | SturmSequence::SturmSequence(const PowerBasisPolynomial1D& polynomial, const DATA_TYPE tolerance) 162 | : _polynomial(polynomial.monicized()), _tolerance(tolerance) 163 | { 164 | const PowerBasisPolynomial1D P0 = polynomial; 165 | const PowerBasisPolynomial1D P1 = polynomial.derivative(); 166 | const PowerBasisPolynomial1D gcdP = PowerBasisPolynomial1D::gcd(P0, P1); 167 | 168 | if (gcdP.degree() != 0) { 169 | this->_polynomial = (P0.divide(gcdP).first).monicized(); 170 | } 171 | 172 | this->_sturmSeqs = this->generateSturmSequences(); 173 | this->_maxDivisions = 1 + static_cast(std::log2(1.f / this->_tolerance)); 174 | } 175 | 176 | template 177 | const std::vector>& SturmSequence::sturmSeqs() const 178 | { 179 | return this->_sturmSeqs; 180 | } 181 | 182 | template int SturmSequence::countSturmSignChanges(DATA_TYPE t) const 183 | { 184 | int signChanges = 0; 185 | 186 | uint32_t previousSign = sgn(this->_polynomial(t)); 187 | 188 | for (auto it = this->_sturmSeqs.cbegin() + 1; it != this->_sturmSeqs.cend(); ++it) { 189 | uint32_t currentSign = sgn(it->operator()(t)); 190 | signChanges += (previousSign ^ currentSign) >> 31; 191 | previousSign = currentSign; 192 | } 193 | 194 | return signChanges; 195 | } 196 | 197 | template 198 | std::vector SturmSequence::solve(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const 199 | { 200 | return solverUtil(lowerBound, upperBound, [this](const DATA_TYPE lowerBound, const DATA_TYPE upperBound) { 201 | return this->solveBisection(lowerBound, upperBound); 202 | }); 203 | } 204 | 205 | template 206 | std::vector SturmSequence::solveLocalMinium(const DATA_TYPE lowerBound, 207 | const DATA_TYPE upperBound) const 208 | { 209 | return solverUtil(lowerBound, upperBound, [this](const DATA_TYPE lowerBound, const DATA_TYPE upperBound) { 210 | return this->solveBisectionLocalMinium(lowerBound, upperBound); 211 | }); 212 | } 213 | 214 | template 215 | DATA_TYPE SturmSequence::solveBisection(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const 216 | { 217 | DATA_TYPE lowerVal = this->_polynomial(lowerBound); 218 | DATA_TYPE upperVal = this->_polynomial(upperBound); 219 | 220 | if (!(sgn(lowerVal) ^ sgn(upperVal))) { 221 | return NAN; 222 | } 223 | 224 | const int maxIterations = this->_maxDivisions; 225 | DATA_TYPE bisectionMin = lowerBound; 226 | DATA_TYPE bisectionMax = upperBound; 227 | 228 | DATA_TYPE mid; 229 | int i = 0; 230 | while ((bisectionMax - bisectionMin) >= this->_tolerance && i < maxIterations) { 231 | mid = (bisectionMax + bisectionMin) / 2.f; 232 | DATA_TYPE midVal = this->_polynomial(mid); 233 | 234 | if (std::fabs(midVal) <= this->_tolerance) { 235 | return mid; 236 | } 237 | 238 | if (sgn(lowerVal) ^ sgn(midVal)) { 239 | bisectionMax = mid; 240 | } else { 241 | lowerVal = midVal; 242 | bisectionMin = mid; 243 | } 244 | ++i; 245 | } 246 | 247 | return bisectionMin; 248 | } 249 | 250 | template 251 | DATA_TYPE SturmSequence::solveBisectionLocalMinium(const DATA_TYPE lowerBound, 252 | const DATA_TYPE upperBound) const 253 | { 254 | DATA_TYPE lowerVal = this->_polynomial(lowerBound); 255 | DATA_TYPE upperVal = this->_polynomial(upperBound); 256 | 257 | if (lowerVal > 0) { 258 | return NAN; 259 | } 260 | 261 | if (upperVal < 0) { 262 | return NAN; 263 | } 264 | 265 | return this->solveBisection(lowerBound, upperBound); 266 | } 267 | 268 | template 269 | std::vector> SturmSequence::generateSturmSequences() const 270 | { 271 | assert(this->_polynomial.coeffs().size() > 0); 272 | 273 | if (this->_polynomial.coeffs().size() == 1) { 274 | return {this->_polynomial}; 275 | } 276 | 277 | std::vector> result; 278 | result.emplace_back(this->_polynomial); 279 | result.emplace_back(this->_polynomial.derivative()); 280 | 281 | PowerBasisPolynomial1D remainderPoly = 282 | (std::prev(std::prev(result.end()))->divide(result.back())).second; 283 | 284 | while (!remainderPoly.isZero()) { 285 | result.emplace_back(remainderPoly.multiply(-1), true); 286 | remainderPoly = (std::prev(std::prev(result.end()))->divide(result.back())).second; 287 | } 288 | 289 | return result; 290 | } 291 | 292 | template 293 | std::pair::Interval>, int> 294 | SturmSequence::separateRoots(const DATA_TYPE lowerBound, const DATA_TYPE upperBound) const 295 | { 296 | std::vector intervalStorage; 297 | intervalStorage.reserve(this->_maxDivisions); 298 | 299 | int lowerBoundSign = this->countSturmSignChanges(lowerBound); 300 | int upperBoundSign = this->countSturmSignChanges(upperBound); 301 | const int totalRoots = lowerBoundSign - upperBoundSign; 302 | int id = 0; 303 | 304 | intervalStorage.emplace_back(lowerBound, upperBound, lowerBoundSign, upperBoundSign, id, totalRoots); 305 | id++; 306 | 307 | std::vector rootIntervals(this->_polynomial.degree()); 308 | int foundRoots = 0; 309 | 310 | while (!intervalStorage.empty() && foundRoots != totalRoots) { 311 | SturmInterval curSI = intervalStorage.back(); 312 | intervalStorage.pop_back(); 313 | 314 | int numRoots = curSI.lowerBoundSign - curSI.upperBoundSign; 315 | 316 | if (numRoots <= 0) { 317 | if (!intervalStorage.empty() && intervalStorage.back().id == curSI.id) { 318 | curSI = intervalStorage.back(); 319 | intervalStorage.pop_back(); 320 | numRoots = curSI.expectedRoots; 321 | } else { 322 | continue; 323 | } 324 | } 325 | 326 | if (numRoots == curSI.expectedRoots && !intervalStorage.empty() && intervalStorage.back().id == curSI.id) { 327 | intervalStorage.pop_back(); 328 | } else if (numRoots == curSI.expectedRoots - 1 && !intervalStorage.empty() && 329 | intervalStorage.back().id == curSI.id) { 330 | rootIntervals[foundRoots++] = intervalStorage.back(); 331 | intervalStorage.pop_back(); 332 | } 333 | 334 | if (numRoots == 1) { 335 | rootIntervals[foundRoots++] = curSI; 336 | } else { 337 | DATA_TYPE mid = (curSI.lowerBound + curSI.upperBound) / 2.f; 338 | if (mid - curSI.lowerBound <= this->_tolerance) { 339 | rootIntervals[foundRoots++] = curSI; 340 | } else { 341 | const int signMid = this->countSturmSignChanges(mid); 342 | 343 | intervalStorage.emplace_back(curSI.lowerBound, mid, curSI.lowerBoundSign, signMid, id, numRoots); 344 | intervalStorage.emplace_back(mid, curSI.upperBound, signMid, curSI.upperBoundSign, id, numRoots); 345 | 346 | id++; 347 | } 348 | } 349 | } 350 | 351 | return std::make_pair(rootIntervals, foundRoots); 352 | } 353 | 354 | template 355 | std::vector 356 | SturmSequence::solverUtil(const DATA_TYPE lowerBound, const DATA_TYPE upperBound, 357 | const std::function& solver) const 358 | { 359 | auto rootIntervalPairs = this->separateRoots(lowerBound, upperBound); 360 | const int foundRoots = rootIntervalPairs.second; 361 | const std::vector& rootIntervals = rootIntervalPairs.first; 362 | 363 | std::vector realRoots; 364 | realRoots.reserve(foundRoots); 365 | 366 | for (size_t i = 0; i < foundRoots; ++i) { 367 | auto root = solver(rootIntervals[i].lowerBound, rootIntervals[i].upperBound); 368 | if (!std::isnan(root)) { 369 | realRoots.emplace_back(root); 370 | } 371 | } 372 | 373 | return realRoots; 374 | } 375 | 376 | } // namespace maths 377 | } // namespace robotics 378 | -------------------------------------------------------------------------------- /include/bezier/Bezier.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Bezier.hpp 3 | * 4 | * @brief Bezier 5 | * 6 | * @author btran 7 | * 8 | * @date 2019-06-19 9 | * 10 | * Copyright (c) organization 11 | * 12 | */ 13 | 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "bezier/NumericalMaths.hpp" 26 | #include "bezier/PowerBasisPolynomial1D.hpp" 27 | #include "bezier/SturmSequence.hpp" 28 | 29 | #ifdef WITH_DEBUG 30 | #define ENABLE_DEBUG 1 31 | #endif 32 | 33 | namespace robotics 34 | { 35 | /** 36 | * @brief Implementation of Bezier Curve 37 | * 38 | * Bezier curves are formed using Bernstern polynomial in the form of r(t) = Sigma(i = 0,n) of b_i * B_(i,n)(t) 39 | * b_i is control points of arbitary dimensions; in this implementation, we focus more on 2d and 3d. 40 | * B_(i,n)(t) is Bernstein polynomial. 41 | * n is the degree of Bezier curve. 42 | * More details should be found here: http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node2.html 43 | */ 44 | template > class Bezier 45 | { 46 | public: 47 | static constexpr T TOLERANCE = 10e-7; 48 | 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | 51 | //! type of control points 52 | using PointType = Container; 53 | 54 | //! type of normal vector 55 | using Normal = PointType; 56 | 57 | //! type of tangent vector 58 | using Tangent = PointType; 59 | 60 | //! vector of control points 61 | using VecPointType = std::vector>; 62 | 63 | //! shared pointer of Bezier type 64 | using Ptr = std::shared_ptr; 65 | 66 | using BoundingBox = std::array; 67 | 68 | /** 69 | * @brief Bezier Constructor 70 | * 71 | * member variable of control points will be initialized with vector of default control points (of zeros) 72 | * 73 | * @param degree of the Bezier curve 74 | */ 75 | explicit Bezier(const size_t degree, const T tolerance = TOLERANCE); 76 | 77 | /** 78 | * @brief Bezier Constructor 79 | * 80 | * @param degree of the Bezier curve 81 | * @param controlPoints control points to initialize member variable 82 | */ 83 | Bezier(const size_t degree, const VecPointType& controlPoints, const T tolerance = TOLERANCE); 84 | 85 | explicit Bezier(const VecPointType& controlPoints, const T tolerance = TOLERANCE); 86 | 87 | /** 88 | * @brief constant getter of control point member variable 89 | * 90 | * @return const VecPointType& 91 | */ 92 | const VecPointType& controlPoints() const; 93 | 94 | /** 95 | * @brief getter for binomial coefficients 96 | * 97 | * @return consts std::vector& 98 | */ 99 | const std::vector& binomialCoeffs() const; 100 | 101 | /** 102 | * @brief Bezier curve (trajectory) approximated from control points 103 | * 104 | * @param numPoints number of points on the trajectory. notice that the first and the last points of this 105 | * trajectory are the first and the last control points. 106 | * @param start the lower bound range of t 107 | * @param end the upper bound range of t 108 | * @return VecPointType 109 | */ 110 | VecPointType trajectory(const size_t numPoints, const double start = 0, const double end = 1); 111 | 112 | /** 113 | * @brief the derivative of current Bezier class 114 | * 115 | * @return Bezier 116 | */ 117 | Bezier derivative() const; 118 | 119 | /** 120 | * @brief the tangent vector at t 121 | * 122 | * @param t t value 123 | * @param normalize boolean value to check whether to normalize the result or not 124 | * @return Tangent 125 | */ 126 | Tangent tangent(const double t, const bool normalize = true) const; 127 | 128 | /** 129 | * @brief the normal vector at t 130 | * 131 | * @param t t value 132 | * @param normalize boolean value to check whether to normalize the result or not 133 | * @return Normal 134 | */ 135 | Normal normal(const double t, const bool normalize = true) const; 136 | 137 | /** 138 | * @brief the curvature at t 139 | * 140 | * @param t t value 141 | * @return double 142 | */ 143 | double curvature(const double t) const; 144 | 145 | /** 146 | * @brief functor to calculate the value of approximated point on Bezier curve at t on the targeted axis 147 | * 148 | * @param axis axis of the point to calculate 149 | * @param t t value 150 | * @return double 151 | */ 152 | double operator()(const size_t axis, const double t) const; 153 | 154 | /** 155 | * @brief functor to calculate the approximated point on Bezier curve at t 156 | * 157 | * @param t t value 158 | * @return PointType 159 | */ 160 | PointType operator()(const double t) const; 161 | 162 | /** 163 | * @brief static function to check if two PointTypes are nearly equal 164 | * 165 | * @param val first PointType 166 | * @param correctVal second PointTypes 167 | * @return bool 168 | */ 169 | static bool fuzzyEquals(const PointType& val, const PointType& correctVal, const T epsilon = TOLERANCE); 170 | 171 | /** 172 | * @brief function to calculate the curve Matrix to transform between a Bezier Bases and a Power Basis 173 | * Though Bezier curve can be calculated as r(t) = Sigma(i = 0,n) of b_i * B_(i,n)(t) 174 | * Also; r(t) = Sigma(i = 0,n) of a_i * t ^ n 175 | * Supposed the b_vector T = (b_0, b_1, ..., b_n) T 176 | * And the a_vector T = (a_0, a_1, ..., a_n) T 177 | * The matrix representation A(mxm) is defined as b_vector = A * a_vector (here m = DEGREE+1) 178 | * We have, A_ij = jCi / jCn for i >= j; 0 otherwise 179 | * 180 | * @return std::vector 181 | */ 182 | std::vector curveMatrix() const; 183 | 184 | /** 185 | * @brief function to calculate the inverse matrix of curve Matrix A 186 | * 187 | * @return std::vector 188 | */ 189 | std::vector inverseCurveMatrix() const; 190 | 191 | /** 192 | * @brief function to extract point data value on each axis 193 | * Supposed a PointType p is a R(n) point. Data value of p on axis i (i = (0,n-1)) is value p[i] 194 | * @param vecPointType vector of points to extract values 195 | * 196 | * @return array of axis-wise point data value vector 197 | */ 198 | std::array, POINT_DIMENSION> extractDataEachAxis(const VecPointType& vecPointType) const; 199 | 200 | std::vector> powerBasisForm() const; 201 | 202 | double closestPointToCurve(const PointType& outliner, const double start = 0, const double end = 1) const; 203 | 204 | BoundingBox estimateBoundingBox(double start = 0, double end = 1) const; 205 | 206 | double approximateLength(const VecPointType& trajectory) const; 207 | 208 | /** 209 | * @brief overloading operator<< to quickly print out the power basis form of bezier curve 210 | * 211 | */ 212 | friend std::ostream& operator<<(std::ostream& os, const Bezier& bezier) 213 | { 214 | std::vector> powerBases = bezier.powerBasisForm(); 215 | 216 | os << "curve function: \n"; 217 | for (size_t i = 0; i < POINT_DIMENSION; ++i) { 218 | os << "dimension number " << i << ": " << powerBases[i] << "\n"; 219 | } 220 | 221 | return os; 222 | } 223 | 224 | private: 225 | /** 226 | * @brief manually-defined cross product function as Eigen's cross product has constraint on dimension. 227 | * 228 | * @param v1 first point 229 | * @param v2 second point 230 | * @return PointType 231 | */ 232 | PointType cross(const PointType& v1, const PointType& v2) const; 233 | 234 | private: 235 | //! control points that define characteristic of Bezier curves 236 | VecPointType _controlPoints; 237 | //! degree of the Bezier curve 238 | const size_t _degree; 239 | //! vector to hold all the (n+1) binomial coefficients of kCn 240 | const std::vector _binomialCoeffs; 241 | 242 | T _tolerance; 243 | }; 244 | 245 | template 246 | typename Bezier::PointType 247 | Bezier::cross(const PointType& v1, const PointType& v2) const 248 | { 249 | if (POINT_DIMENSION != 3) { 250 | throw std::out_of_range("this method is for point of 3 dimensions"); 251 | } 252 | 253 | PointType result; 254 | result << (v1[1] * v2[2] - v1[2] * v2[1]), (v1[2] * v2[0] - v1[0] * v2[2]), (v1[0] * v2[1] - v1[1] * v2[0]); 255 | 256 | return result; 257 | } 258 | 259 | template 260 | Bezier::Bezier(const size_t degree, const T tolerance) 261 | : _controlPoints(VecPointType(degree + 1, static_cast(PointType::Zero()))), _degree(degree), 262 | _tolerance(tolerance), _binomialCoeffs(maths::binomialCoeffs(degree)) 263 | { 264 | } 265 | 266 | template 267 | Bezier::Bezier(const size_t degree, const VecPointType& controlPoints, const T tolerance) 268 | : _controlPoints(controlPoints), _degree(degree), _tolerance(tolerance), 269 | _binomialCoeffs(maths::binomialCoeffs(degree)) 270 | { 271 | } 272 | 273 | template 274 | Bezier::Bezier(const VecPointType& controlPoints, const T tolerance) 275 | : _controlPoints(controlPoints), _degree(controlPoints.size() - 1), _tolerance(tolerance), 276 | _binomialCoeffs(maths::binomialCoeffs(controlPoints.size() - 1)) 277 | { 278 | assert(controlPoints.size() > 0); 279 | } 280 | 281 | template 282 | const typename Bezier::VecPointType& 283 | Bezier::controlPoints() const 284 | { 285 | return this->_controlPoints; 286 | } 287 | 288 | template 289 | const std::vector& Bezier::binomialCoeffs() const 290 | { 291 | return this->_binomialCoeffs; 292 | } 293 | 294 | template 295 | typename Bezier::VecPointType 296 | Bezier::trajectory(const size_t numPoints, const double start, const double end) 297 | { 298 | if (numPoints == 0) { 299 | throw std::out_of_range("Number of points must be more than 0"); 300 | } 301 | 302 | VecPointType v; 303 | v.reserve(numPoints + 1); 304 | 305 | double delta = (end - start) / numPoints; 306 | for (size_t i = 0; i <= numPoints; ++i) { 307 | double t = start + i * delta; 308 | v.emplace_back(this->operator()(t)); 309 | } 310 | 311 | return v; 312 | } 313 | 314 | template 315 | Bezier Bezier::derivative() const 316 | { 317 | if (this->_degree == 0) { 318 | throw std::out_of_range("degree must be more than 0"); 319 | } 320 | 321 | VecPointType deriBControlPoints; 322 | deriBControlPoints.reserve(this->_degree); 323 | for (size_t i = 0; i < this->_degree; ++i) { 324 | deriBControlPoints.emplace_back(this->_degree * (this->_controlPoints[i + 1] - this->_controlPoints[i])); 325 | } 326 | 327 | return Bezier(this->_degree - 1, deriBControlPoints); 328 | } 329 | 330 | template 331 | typename Bezier::Tangent 332 | Bezier::tangent(const double t, const bool normalize) const 333 | { 334 | Tangent tag; 335 | Bezier derivativeBezier = this->derivative(); 336 | tag = derivativeBezier(t); 337 | if (normalize) { 338 | tag.normalize(); 339 | } 340 | 341 | return tag; 342 | } 343 | 344 | template 345 | typename Bezier::Normal 346 | Bezier::normal(const double t, const bool normalize) const 347 | { 348 | if (POINT_DIMENSION != 2 && POINT_DIMENSION != 3) { 349 | throw std::out_of_range("This method is for control points of dimension of 2 or 3"); 350 | } 351 | 352 | Tangent tag = this->tangent(t, normalize); 353 | 354 | Normal nor; 355 | 356 | if (POINT_DIMENSION == 2) { 357 | nor << -(tag[1]), tag[0]; 358 | } 359 | 360 | if (POINT_DIMENSION == 3) { 361 | // Here is the naive implementation for 3d normal 362 | // Algorithm approach can be Rotation Minimising Frames 363 | // https://pomax.github.io/bezierinfo/#pointvectors 364 | 365 | Bezier secondDerivBezier = this->derivative().derivative(); 366 | 367 | PointType b = static_cast((tag + secondDerivBezier(t)).normalized()); 368 | 369 | PointType r = static_cast(this->cross(b, tag).normalized()); 370 | 371 | nor = this->cross(r, tag); 372 | 373 | if (normalize) { 374 | nor.normalize(); 375 | } 376 | } 377 | 378 | return nor; 379 | } 380 | 381 | template 382 | double Bezier::curvature(const double t) const 383 | { 384 | if (this->_degree < 2 || (POINT_DIMENSION != 2 && POINT_DIMENSION != 3)) { 385 | std::string msg = "This method is for degree more than 1 and"; 386 | msg += " control points for dimension of 2 or 3"; 387 | throw std::out_of_range(msg); 388 | } 389 | 390 | Bezier derivativeBezier = this->derivative(); 391 | Bezier secondDerivBezier = derivativeBezier.derivative(); 392 | 393 | PointType a = derivativeBezier(t); 394 | PointType b = secondDerivBezier(t); 395 | double curvature; 396 | 397 | if (POINT_DIMENSION == 2) { 398 | curvature = (a[0] * b[1] - a[1] * b[0]) / pow(a.norm(), 3); 399 | } 400 | 401 | if (POINT_DIMENSION == 3) { 402 | curvature = (this->cross(a, b).norm() / pow(a.norm(), 3)); 403 | } 404 | 405 | return curvature; 406 | } 407 | 408 | template 409 | double Bezier::operator()(const size_t axis, const double t) const 410 | { 411 | if (axis >= POINT_DIMENSION) { 412 | throw std::out_of_range("axis out of range"); 413 | } 414 | 415 | if (this->_controlPoints.size() < this->_degree + 1) { 416 | throw std::out_of_range("number of control points must be more than degree"); 417 | } 418 | 419 | std::vector polynominalCoeffs = maths::polynomialCoeffs(this->_degree, t); 420 | 421 | double sum = 0; 422 | for (size_t i = 0; i < this->_degree + 1; ++i) { 423 | sum += this->_controlPoints[i][axis] * _binomialCoeffs[i] * polynominalCoeffs[i]; 424 | } 425 | 426 | return sum; 427 | } 428 | 429 | template 430 | Container Bezier::operator()(const double t) const 431 | { 432 | if (this->controlPoints().empty()) { 433 | throw std::out_of_range("No control points"); 434 | } 435 | 436 | PointType p; 437 | 438 | for (size_t i = 0; i < POINT_DIMENSION; ++i) { 439 | p[i] = static_cast(this->operator()(i, t)); 440 | } 441 | 442 | return p; 443 | } 444 | 445 | template 446 | bool Bezier::fuzzyEquals(const PointType& val, const PointType& correctVal, 447 | const T epsilon) 448 | { 449 | for (size_t i = 0; i < val.size(); ++i) { 450 | if (!maths::combinedToleranceEquals(val[i], correctVal[i], epsilon)) { 451 | return false; 452 | } 453 | } 454 | 455 | return true; 456 | } 457 | 458 | template 459 | std::vector Bezier::curveMatrix() const 460 | { 461 | const size_t WIDTH = this->_degree + 1, HEIGHT = this->_degree + 1; 462 | std::vector result(HEIGHT * WIDTH, 0.0); 463 | 464 | for (size_t i = 0; i < HEIGHT; ++i) { 465 | for (size_t j = 0; j < WIDTH; ++j) { 466 | if (i >= j) { 467 | result[i * WIDTH + j] = maths::binomialCoeff(i, j) / maths::binomialCoeff(this->_degree, j); 468 | } 469 | } 470 | } 471 | 472 | return result; 473 | } 474 | 475 | template 476 | std::vector Bezier::inverseCurveMatrix() const 477 | { 478 | const size_t WIDTH = this->_degree + 1, HEIGHT = this->_degree + 1; 479 | std::vector result(HEIGHT * WIDTH, 0.0); 480 | 481 | for (size_t i = 0; i < HEIGHT; ++i) { 482 | for (size_t j = 0; j < WIDTH; ++j) { 483 | if (i >= j) { 484 | result[i * WIDTH + j] = 485 | std::pow(-1, i - j) * maths::binomialCoeff(i, j) * maths::binomialCoeff(this->_degree, i); 486 | } 487 | } 488 | } 489 | 490 | return result; 491 | } 492 | 493 | template 494 | std::array, POINT_DIMENSION> 495 | Bezier::extractDataEachAxis(const VecPointType& vecPointType) const 496 | { 497 | std::array, POINT_DIMENSION> result; 498 | 499 | for (size_t i = 0; i < POINT_DIMENSION; ++i) { 500 | std::vector curV; 501 | curV.reserve(vecPointType.size()); 502 | std::transform(vecPointType.begin(), vecPointType.end(), std::back_inserter(curV), 503 | [&i](const PointType& p) { return p[i]; }); 504 | result[i] = curV; 505 | } 506 | 507 | return result; 508 | } 509 | 510 | template 511 | std::vector> Bezier::powerBasisForm() const 512 | { 513 | assert(this->controlPoints().size() == this->_degree + 1); 514 | 515 | const size_t WIDTH = this->_degree + 1, HEIGHT = this->_degree + 1; 516 | 517 | const std::vector curMatrix = this->inverseCurveMatrix(); 518 | 519 | VecPointType powerBasisCoeffsVec; 520 | powerBasisCoeffsVec.reserve(this->_degree + 1); 521 | 522 | for (size_t i = 0; i < HEIGHT; ++i) { 523 | PointType curPoints = static_cast(PointType::Zero()); 524 | for (size_t j = 0; j < WIDTH; ++j) { 525 | curPoints += curMatrix[i * WIDTH + j] * this->controlPoints()[j]; 526 | } 527 | powerBasisCoeffsVec.emplace_back(curPoints); 528 | } 529 | 530 | auto powerBasisCoeffs = this->extractDataEachAxis(powerBasisCoeffsVec); 531 | std::vector> result; 532 | result.reserve(POINT_DIMENSION); 533 | 534 | for (const auto& powerBasisCoeff : powerBasisCoeffs) { 535 | result.emplace_back(maths::PowerBasisPolynomial1D(powerBasisCoeff)); 536 | } 537 | 538 | return result; 539 | } 540 | 541 | template 542 | double Bezier::closestPointToCurve(const PointType& outliner, const double start, 543 | const double end) const 544 | { 545 | // solve P(t) = (Q(t) - outliner).dot(Q'(t)') = 0 546 | // Q1(t) = (Q(t) - outliner) 547 | // Q2(t) = Q'(t) 548 | 549 | this->_controlPoints; 550 | VecPointType q1ControlPoints; 551 | q1ControlPoints.reserve(this->_controlPoints.size()); 552 | std::transform(this->_controlPoints.begin(), this->_controlPoints.end(), std::back_inserter(q1ControlPoints), 553 | [&outliner](const PointType& p) { return p - outliner; }); 554 | 555 | const Bezier Q1(this->_degree, q1ControlPoints); 556 | const Bezier Q2 = this->derivative(); 557 | 558 | const auto q1BasisPowerForm = Q1.powerBasisForm(); 559 | const auto q2BasisPowerForm = Q2.powerBasisForm(); 560 | maths::PowerBasisPolynomial1D poly({0}, false, this->_tolerance); 561 | 562 | for (size_t i = 0; i < POINT_DIMENSION; ++i) { 563 | poly += q1BasisPowerForm[i].multiply(q2BasisPowerForm[i]); 564 | } 565 | 566 | const maths::SturmSequence ss(poly, this->_tolerance); 567 | 568 | const auto roots = ss.solveLocalMinium(this->_tolerance, 1.f - this->_tolerance); 569 | 570 | std::vector shortestVals; 571 | std::vector shortestDistances; 572 | shortestVals.reserve(roots.size() + 2); 573 | shortestDistances.reserve(roots.size() + 2); 574 | 575 | shortestVals.emplace_back(start); 576 | shortestDistances.emplace_back((this->operator()(start) - outliner).norm()); 577 | 578 | for (const auto& root : roots) { 579 | shortestVals.emplace_back(root); 580 | shortestDistances.emplace_back((this->operator()(root) - outliner).norm()); 581 | } 582 | 583 | shortestVals.emplace_back(end); 584 | shortestDistances.emplace_back((this->operator()(end) - outliner).norm()); 585 | 586 | int shortestValIndx = 587 | std::distance(shortestDistances.begin(), std::min_element(shortestDistances.begin(), shortestDistances.end())); 588 | 589 | return shortestVals[shortestValIndx]; 590 | } 591 | 592 | template 593 | typename Bezier::BoundingBox 594 | Bezier::estimateBoundingBox(double start, double end) const 595 | { 596 | auto derivPowerBases = this->derivative().powerBasisForm(); 597 | PointType topLeft, bottomRight; 598 | 599 | for (size_t i = 0; i < POINT_DIMENSION; ++i) { 600 | const maths::PowerBasisPolynomial1D axisPoly = derivPowerBases[i]; 601 | const maths::SturmSequence ss(axisPoly, this->_tolerance); 602 | const auto roots = ss.solve(start + this->_tolerance, end - this->_tolerance); 603 | std::vector axisVals; 604 | axisVals.reserve(roots.size() + 2); 605 | 606 | axisVals.emplace_back(this->operator()(i, start)); 607 | 608 | std::transform(roots.begin(), roots.end(), std::back_inserter(axisVals), 609 | [&i, this](const T val) { return this->operator()(i, val); }); 610 | 611 | axisVals.emplace_back(this->operator()(i, end)); 612 | 613 | const auto minMax = std::minmax_element(axisVals.begin(), axisVals.end()); 614 | topLeft[i] = *minMax.first; 615 | bottomRight[i] = *minMax.second; 616 | } 617 | 618 | return {topLeft, bottomRight}; 619 | } 620 | 621 | template 622 | double Bezier::approximateLength(const VecPointType& trajectory) const 623 | { 624 | double length = 0; 625 | for (auto it = trajectory.cbegin(); it != trajectory.cend() - 1; ++it) { 626 | length += (*it - *std::next(it)).norm(); 627 | } 628 | 629 | return length; 630 | } 631 | 632 | } // namespace robotics 633 | --------------------------------------------------------------------------------