Fixed off by one.
@@ -150,8 +150,9 @@ size_t PiecewisePolyPath::findSegmentIndex(value_type pos) const {
<< ", " << m_breakpoints[m_breakpoints.size()-1] << ']';
throw std::runtime_error(oss.str());
}
- auto it = std::lower_bound(m_breakpoints.begin(), m_breakpoints.end(), pos);
- return std::min(static_cast<size_t>(std::distance(m_breakpoints.begin(), it)), m_coefficients.size() - 1);
+ auto it = std::upper_bound(m_breakpoints.begin(), m_breakpoints.end(), pos);
+ auto idx = std::distance(m_breakpoints.begin(), it)-1;
+ return std::min(static_cast<size_t>(std::max(idx, long{0})), m_coefficients.size() - 1);
}
void PiecewisePolyPath::checkInputArgs() {
