diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 791de8a242..25b9f9258a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -84,4 +84,17 @@ public final class MathUtil { public static double angleModulus(double angleRadians) { return inputModulus(angleRadians, -Math.PI, Math.PI); } + + /** + * Perform linear interpolation between two values. + * + * @param startValue The value to start at. + * @param endValue The value to end at. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @SuppressWarnings("ParameterName") + public static double interpolate(double startValue, double endValue, double t) { + return startValue + (endValue - startValue) * MathUtil.clamp(t, 0, 1); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 6033b898f1..f64eff41a0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -8,12 +8,13 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; /** Represents a 2d pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) -public class Pose2d { +public class Pose2d implements Interpolatable { private final Translation2d m_translation; private final Rotation2d m_rotation; @@ -242,4 +243,18 @@ public class Pose2d { public int hashCode() { return Objects.hash(m_translation, m_rotation); } + + @Override + @SuppressWarnings("ParameterName") + public Pose2d interpolate(Pose2d endValue, double t) { + if (t < 0) { + return this; + } else if (t >= 1) { + return endValue; + } else { + var twist = this.log(endValue); + var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t); + return this.exp(scaledTwist); + } + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 74ef228919..08e54387dd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -8,12 +8,14 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; /** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) -public class Rotation2d { +public class Rotation2d implements Interpolatable { private final double m_value; private final double m_cos; private final double m_sin; @@ -198,4 +200,10 @@ public class Rotation2d { public int hashCode() { return Objects.hash(m_value); } + + @Override + @SuppressWarnings("ParameterName") + public Rotation2d interpolate(Rotation2d endValue, double t) { + return new Rotation2d(MathUtil.interpolate(this.getRadians(), endValue.getRadians(), t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 251c078671..84eaea8ccb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -8,6 +8,8 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; /** @@ -20,7 +22,7 @@ import java.util.Objects; @SuppressWarnings({"ParameterName", "MemberName"}) @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) -public class Translation2d { +public class Translation2d implements Interpolatable { private final double m_x; private final double m_y; @@ -196,4 +198,11 @@ public class Translation2d { public int hashCode() { return Objects.hash(m_x, m_y); } + + @Override + public Translation2d interpolate(Translation2d endValue, double t) { + return new Translation2d( + MathUtil.interpolate(this.getX(), endValue.getX(), t), + MathUtil.interpolate(this.getY(), endValue.getY(), t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java new file mode 100644 index 0000000000..f10820f45a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolatable.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.interpolation; + +/** + * An object should extend interpolatable if you wish to interpolate between a lower and upper + * bound, such as a robot position on the field between timesteps. This behavior can be linear or + * nonlinear. + * + * @param The class that is interpolatable. + */ +public interface Interpolatable { + /** + * Return the interpolated value. This object is assumed to be the starting position, or lower + * bound. + * + * @param endValue The upper bound, or end. + * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. + * @return The interpolated value. + */ + @SuppressWarnings("ParameterName") + T interpolate(T endValue, double t); +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java new file mode 100644 index 0000000000..676bd7c7f7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java @@ -0,0 +1,149 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.interpolation; + +import edu.wpi.first.math.MathUtil; +import java.util.NavigableMap; +import java.util.TreeMap; + +/** + * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application + * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot + * pose at the time when vision or other global measurement were recorded is necessary, or for + * recording the past angles of mechanisms as measured by encoders. + * + * @param The type stored in this buffer. + */ +public class TimeInterpolatableBuffer { + private final double m_historySize; + private final InterpolateFunction m_interpolatingFunc; + private final NavigableMap m_buffer = new TreeMap<>(); + + private TimeInterpolatableBuffer( + InterpolateFunction interpolateFunction, double historySizeSeconds) { + this.m_historySize = historySizeSeconds; + this.m_interpolatingFunc = interpolateFunction; + } + + /** + * Create a new TimeInterpolatableBuffer. + * + * @param interpolateFunction The function used to interpolate between values. + * @param historySizeSeconds The history size of the buffer. + * @param The type of data to store in the buffer. + * @return The new TimeInterpolatableBuffer. + */ + public static TimeInterpolatableBuffer createBuffer( + InterpolateFunction interpolateFunction, double historySizeSeconds) { + return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds); + } + + /** + * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}. + * + * @param historySizeSeconds The history size of the buffer. + * @param The type of {@link Interpolatable} to store in the buffer. + * @return The new TimeInterpolatableBuffer. + */ + public static > TimeInterpolatableBuffer createBuffer( + double historySizeSeconds) { + return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds); + } + + /** + * Create a new TimeInterpolatableBuffer to store Double values. + * + * @param historySizeSeconds The history size of the buffer. + * @return The new TimeInterpolatableBuffer. + */ + public static TimeInterpolatableBuffer createDoubleBuffer(double historySizeSeconds) { + return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds); + } + + /** + * Add a sample to the buffer. + * + * @param timeSeconds The timestamp of the sample. + * @param sample The sample object. + */ + public void addSample(double timeSeconds, T sample) { + cleanUp(timeSeconds); + m_buffer.put(timeSeconds, sample); + } + + /** + * Removes samples older than our current history size. + * + * @param time The current timestamp. + */ + private void cleanUp(double time) { + while (!m_buffer.isEmpty()) { + var entry = m_buffer.firstEntry(); + if (time - entry.getKey() >= m_historySize) { + m_buffer.remove(entry.getKey()); + } else { + return; + } + } + } + + /** Clear all old samples. */ + public void clear() { + m_buffer.clear(); + } + + /** + * Sample the buffer at the given time. If the buffer is empty, this will return null. + * + * @param timeSeconds The time at which to sample. + * @return The interpolated value at that timestamp. Might be null. + */ + @SuppressWarnings("UnnecessaryParentheses") + public T getSample(double timeSeconds) { + if (m_buffer.isEmpty()) { + return null; + } + + // Special case for when the requested time is the same as a sample + var nowEntry = m_buffer.get(timeSeconds); + if (nowEntry != null) { + return nowEntry; + } + + var topBound = m_buffer.ceilingEntry(timeSeconds); + var bottomBound = m_buffer.floorEntry(timeSeconds); + + // Return null if neither sample exists, and the opposite bound if the other is null + if (topBound == null && bottomBound == null) { + return null; + } else if (topBound == null) { + return bottomBound.getValue(); + } else if (bottomBound == null) { + return topBound.getValue(); + } else { + // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference + // between the current time and bottom bound) and (the difference between top and bottom + // bounds). + return m_interpolatingFunc.interpolate( + bottomBound.getValue(), + topBound.getValue(), + ((timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))); + } + } + + public interface InterpolateFunction { + /** + * Return the interpolated value. This object is assumed to be the starting position, or lower + * bound. + * + * @param start The lower bound, or start. + * @param end The upper bound, or end. + * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. + * @return The interpolated value. + */ + @SuppressWarnings("ParameterName") + T interpolate(T start, T end, double t); + } +} diff --git a/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp b/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp new file mode 100644 index 0000000000..f594cd6629 --- /dev/null +++ b/wpimath/src/main/native/cpp/interpolation/TimeInterpolatableBuffer.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/interpolation/TimeInterpolatableBuffer.h" + +namespace frc { + +// Template specialization to ensure that Pose2d uses pose exponential +template <> +TimeInterpolatableBuffer::TimeInterpolatableBuffer( + units::second_t historySize) + : m_historySize(historySize), + m_interpolatingFunc([](const Pose2d& start, const Pose2d& end, double t) { + if (t < 0) { + return start; + } else if (t >= 1) { + return end; + } else { + Twist2d twist = start.Log(end); + Twist2d scaledTwist = twist * t; + return start.Exp(scaledTwist); + } + }) {} + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index db419f7849..ecb36c2b80 100644 --- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -6,6 +6,7 @@ #include +#include #include #include "units/math.h" @@ -25,7 +26,7 @@ bool Trajectory::State::operator!=(const Trajectory::State& other) const { Trajectory::State Trajectory::State::Interpolate(State endValue, double i) const { // Find the new [t] value. - const auto newT = Lerp(t, endValue.t, i); + const auto newT = wpi::Lerp(t, endValue.t, i); // Find the delta time between the current state and the interpolated state. const auto deltaT = newT - t; @@ -58,8 +59,8 @@ Trajectory::State Trajectory::State::Interpolate(State endValue, newS / endValue.pose.Translation().Distance(pose.Translation()); return {newT, newV, acceleration, - Lerp(pose, endValue.pose, interpolationFrac), - Lerp(curvature, endValue.curvature, interpolationFrac)}; + wpi::Lerp(pose, endValue.pose, interpolationFrac), + wpi::Lerp(curvature, endValue.curvature, interpolationFrac)}; } Trajectory::Trajectory(const std::vector& states) : m_states(states) { diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 9d7a856b56..24d4d7e62c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -53,5 +53,15 @@ struct WPILIB_DLLEXPORT Twist2d { * @return Whether the two objects are not equal. */ bool operator!=(const Twist2d& other) const { return !operator==(other); } + + /** + * Scale this by a given factor. + * + * @param factor The factor by which to scale. + * @return The scaled Twist2d. + */ + Twist2d operator*(double factor) { + return Twist2d{dx * factor, dy * factor, dtheta * factor}; + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h new file mode 100644 index 0000000000..a049e40875 --- /dev/null +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -0,0 +1,128 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "frc/geometry/Pose2d.h" +#include "units/time.h" + +namespace frc { + +/** + * The TimeInterpolatableBuffer provides an easy way to estimate past + * measurements. One application might be in conjunction with the + * DifferentialDrivePoseEstimator, where knowledge of the robot pose at the time + * when vision or other global measurement were recorded is necessary, or for + * recording the past angles of mechanisms as measured by encoders. + * + * When sampling this buffer, a user-provided function or wpi::Lerp can be + * used. For Pose2ds, we use Twists. + * + * @tparam T The type stored in this buffer. + */ +template +class TimeInterpolatableBuffer { + public: + /** + * Create a new TimeInterpolatableBuffer. + * + * @param historySize The history size of the buffer. + * @param func The function used to interpolate between values. + */ + TimeInterpolatableBuffer(units::second_t historySize, + std::function func) + : m_historySize(historySize), m_interpolatingFunc(func) {} + + /** + * Create a new TimeInterpolatableBuffer. By default, the interpolation + * function is wpi::Lerp except for Pose2d, which uses the pose exponential. + * + * @param historySize The history size of the buffer. + */ + explicit TimeInterpolatableBuffer(units::second_t historySize) + : m_historySize(historySize), + m_interpolatingFunc([](const T& start, const T& end, double t) { + return wpi::Lerp(start, end, t); + }) {} + + /** + * Add a sample to the buffer. + * + * @param time The timestamp of the sample. + * @param sample The sample object. + */ + void AddSample(units::second_t time, T sample) { + // Add the new state into the vector. + if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) { + m_pastSnapshots.emplace_back(time, sample); + } else { + m_pastSnapshots.insert( + std::upper_bound( + m_pastSnapshots.begin(), m_pastSnapshots.end(), time, + [](auto t, const auto& pair) { return t < pair.first; }), + std::pair(time, sample)); + } + while (time - m_pastSnapshots[0].first > m_historySize) { + m_pastSnapshots.erase(m_pastSnapshots.begin()); + } + } + + /** Clear all old samples. */ + void Clear() { m_pastSnapshots.clear(); } + + /** + * Sample the buffer at the given time. If there are no elements in the + * buffer, calling this function results in undefined behavior. + * + * @param time The time at which to sample the buffer. + */ + T Sample(units::second_t time) { + // We will perform a binary search to find the index of the element in the + // vector that has a timestamp that is equal to or greater than the vision + // measurement timestamp. + + if (time <= m_pastSnapshots.front().first) { + return m_pastSnapshots.front().second; + } + if (time > m_pastSnapshots.back().first) { + return m_pastSnapshots.back().second; + } + if (m_pastSnapshots.size() < 2) { + return m_pastSnapshots[0].second; + } + + // Get the iterator which has a key no less than the requested key. + auto upper_bound = std::lower_bound( + m_pastSnapshots.begin(), m_pastSnapshots.end(), time, + [](const auto& pair, auto t) { return t > pair.first; }); + + auto lower_bound = upper_bound - 1; + + double t = ((time - lower_bound->first) / + (upper_bound->first - lower_bound->first)); + + return m_interpolatingFunc(lower_bound->second, upper_bound->second, t); + } + + private: + units::second_t m_historySize; + std::vector> m_pastSnapshots; + std::function m_interpolatingFunc; +}; + +// Template specialization to ensure that Pose2d uses pose exponential +template <> +WPILIB_DLLEXPORT TimeInterpolatableBuffer::TimeInterpolatableBuffer( + units::second_t historySize); + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h index 2fad345f02..d071ca36a3 100644 --- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -157,20 +157,6 @@ class WPILIB_DLLEXPORT Trajectory { private: std::vector m_states; units::second_t m_totalTime = 0_s; - - /** - * Linearly interpolates between two values. - * - * @param startValue The start value. - * @param endValue The end value. - * @param t The fraction for interpolation. - * - * @return The interpolated value. - */ - template - static T Lerp(const T& startValue, const T& endValue, const double t) { - return startValue + (endValue - startValue) * t; - } }; WPILIB_DLLEXPORT diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java new file mode 100644 index 0000000000..f1c3f83b4b --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.interpolation; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.junit.jupiter.api.Test; + +public class TimeInterpolatableBufferTest { + @Test + public void testInterpolation() { + TimeInterpolatableBuffer buffer = TimeInterpolatableBuffer.createBuffer(10); + + buffer.addSample(0, new Rotation2d()); + assertEquals(0, buffer.getSample(0).getRadians(), 0.001); + buffer.addSample(1, new Rotation2d(1)); + assertEquals(0.5, buffer.getSample(0.5).getRadians(), 0.001); + assertEquals(1.0, buffer.getSample(1.0).getRadians(), 0.001); + buffer.addSample(3, new Rotation2d(2)); + assertEquals(1.5, buffer.getSample(2).getRadians(), 0.001); + + buffer.addSample(10.5, new Rotation2d(2)); + assertEquals(new Rotation2d(1), buffer.getSample(0)); + } + + @Test + public void testPose2d() { + TimeInterpolatableBuffer buffer = TimeInterpolatableBuffer.createBuffer(10); + + // We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5 + buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90))); + buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0))); + Pose2d sample = buffer.getSample(0.5); + + assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01); + assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01); + assertEquals(45, sample.getRotation().getDegrees(), 0.01); + } +} diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp new file mode 100644 index 0000000000..ee2ec9b102 --- /dev/null +++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "gtest/gtest.h" +#include "units/time.h" + +TEST(TimeInterpolatableBufferTest, Interpolation) { + frc::TimeInterpolatableBuffer buffer{10_s}; + + buffer.AddSample(0_s, frc::Rotation2d(0_rad)); + EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad)); + buffer.AddSample(1_s, frc::Rotation2d(1_rad)); + EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad)); + EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad)); + buffer.AddSample(3_s, frc::Rotation2d(2_rad)); + EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad)); + + buffer.AddSample(10.5_s, frc::Rotation2d(2_rad)); + EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad)); +} + +TEST(TimeInterpolatableBufferTest, Pose2d) { + frc::TimeInterpolatableBuffer buffer{10_s}; + // We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5 + buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg}); + buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg}); + frc::Pose2d sample = buffer.Sample(0.5_s); + EXPECT_TRUE(std::abs(sample.X().to() - (1 - 1 / std::sqrt(2))) < + 0.01); + EXPECT_TRUE(std::abs(sample.Y().to() - (1 / std::sqrt(2))) < 0.01); + EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to() - 45) < 0.01); +} diff --git a/wpiutil/src/main/native/include/wpi/MathExtras.h b/wpiutil/src/main/native/include/wpi/MathExtras.h index 52a56a1ae5..ac88cb96da 100644 --- a/wpiutil/src/main/native/include/wpi/MathExtras.h +++ b/wpiutil/src/main/native/include/wpi/MathExtras.h @@ -845,6 +845,20 @@ constexpr int sgn(T val) { return (T(0) < val) - (val < T(0)); } +/** + * Linearly interpolates between two values. + * + * @param startValue The start value. + * @param endValue The end value. + * @param t The fraction for interpolation. + * + * @return The interpolated value. + */ +template +constexpr T Lerp(const T& startValue, const T& endValue, double t) { + return startValue + (endValue - startValue) * t; +} + } // namespace wpi #endif