Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use std::lower_bound in Trajectory::Sample() calculation (#2774)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-10-08 23:09:30 -04:00
committed by GitHub
parent b0ee11f7cc
commit 37643ab0b2
1 changed files with 13 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -7,6 +7,8 @@
#include "frc/trajectory/Trajectory.h"
#include <algorithm>
#include <wpi/json.h>
#include "units/math.h"
@@ -69,44 +71,27 @@ Trajectory::State Trajectory::Sample(units::second_t t) const {
if (t <= m_states.front().t) return m_states.front();
if (t >= m_totalTime) return m_states.back();
// To get the element that we want, we will use a binary search algorithm
// instead of iterating over a for-loop. A binary search is O(std::log(n))
// whereas searching using a loop is O(n).
// Use binary search to get the element with a timestamp no less than the
// requested timestamp. This starts at 1 because we use the previous state
// later on for interpolation.
auto sample =
std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t,
[](const auto& a, const auto& b) { return a.t < b; });
// This starts at 1 because we use the previous state later on for
// interpolation.
int low = 1;
int high = m_states.size() - 1;
while (low != high) {
int mid = (low + high) / 2;
if (m_states[mid].t < t) {
// This index and everything under it are less than the requested
// timestamp. Therefore, we can discard them.
low = mid + 1;
} else {
// t is at least as large as the element at this index. This means that
// anything after it cannot be what we are looking for.
high = mid;
}
}
// High and Low should be the same.
auto prevSample = sample - 1;
// The sample's timestamp is now greater than or equal to the requested
// timestamp. If it is greater, we need to interpolate between the
// previous state and the current state to get the exact state that we
// want.
const auto sample = m_states[low];
const auto prevSample = m_states[low - 1];
// If the difference in states is negligible, then we are spot on!
if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
return sample;
if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) {
return *sample;
}
// Interpolate between the two states for the state that we want.
return prevSample.Interpolate(sample,
(t - prevSample.t) / (sample.t - prevSample.t));
return prevSample->Interpolate(
*sample, (t - prevSample->t) / (sample->t - prevSample->t));
}
Trajectory Trajectory::TransformBy(const Transform2d& transform) {