Replace for loop in piecewise poly path to find index by std::lower_bound.

hungpham2511 / toppra

robotic motion planning library

https://hungpham2511.github.io/toppra/index.html

MIT License

612 stars 169 forks source link

Replace for loop in piecewise poly path to find index by std::lower_bound. #177

Closed jakob-ludwiger closed 3 years ago

jakob-ludwiger commented 3 years ago

Since the breakpoints in the piecewise poly path are sorted, it should be possible to use std::uppler_bound which is way more efficient.

Changes in this PRs:

Replace for loop in piecewise poly path to find index by std::lower_bound.

Checklists:

[x] Update HISTORY.md with a single line describing this PR

hungpham2511 commented 3 years ago

Could you have a look at the CI failure? Look like either the test is wrong or the new code introducs a bug.

/home/circleci/repo/cpp/tests/test_poly_geometric_path.cpp:77: Failure
Expected equality of these values:
  positions[0](0)
    Which is: 2.25
  1 * pow(0.5, 2) + 2 * pow(0.5, 1) + 3
    Which is: 4.25

jakob-ludwiger commented 3 years ago

I fixed an off by one error. Sorry for not checking the test results.

hungpham2511 commented 3 years ago

Look good to me. Thank you both for submitting and for reviewing the PR!