diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index e52e39f51c..067b8de068 100644
--- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -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) {