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

[wpimath] MathUtil: Add 2D variants of applyDeadband and copySignPow (#8057)

Browse Source
This commit is contained in:
Michael Lesirge
2025-10-10 13:44:12 -07:00
committed by GitHub
parent 4c4996e638
commit 2b43541b94
4 changed files with 432 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -107,6 +107,39 @@ public final class MathUtil {
return applyDeadband(value, deadband, 1);
}
/**
* Returns a zero vector if the given vector is within the specified distance from the origin. The
* remaining distance between the deadband and the maximum distance is scaled from the origin to
* the maximum distance.
*
* @param value Value to clip.
* @param deadband Distance from origin.
* @param maxMagnitude The maximum distance from the origin of the input. Can be infinite.
* @param <R> The number of rows in the vector.
* @return The value after the deadband is applied.
*/
public static <R extends Num> Vector<R> applyDeadband(
Vector<R> value, double deadband, double maxMagnitude) {
if (value.norm() < 1e-9) {
return value.times(0);
}
return value.unit().times(applyDeadband(value.norm(), deadband, maxMagnitude));
}
/**
* Returns a zero vector if the given vector is within the specified distance from the origin. The
* remaining distance between the deadband and a distance of 1.0 is scaled from the origin to a
* distance of 1.0.
*
* @param value Value to clip.
* @param deadband Distance from origin.
* @param <R> The number of rows in the vector.
* @return The value after the deadband is applied.
*/
public static <R extends Num> Vector<R> applyDeadband(Vector<R> value, double deadband) {
return applyDeadband(value, deadband, 1);
}
/**
* Raises the input to the power of the given exponent while preserving its sign.
*
@@ -143,6 +176,42 @@ public final class MathUtil {
return copySignPow(value, exponent, 1);
}
/**
* Raises the norm of the input to the power of the given exponent while preserving its direction.
*
* <p>The function normalizes the norm of the input to the range [0, 1] based on the maximum
* distance, raises it to the power of the exponent, then scales the result back to the original
* range. This keeps the value in the original max distance and gives consistent curve behavior
* regardless of the input norm's scale.
*
* @param value The input vector to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be
* positive.
* @param maxMagnitude The maximum expected distance from origin of input. Must be positive.
* @param <R> The number of rows in the vector.
* @return The transformed value with the same direction and norm scaled to the input range.
*/
public static <R extends Num> Vector<R> copyDirectionPow(
Vector<R> value, double exponent, double maxMagnitude) {
if (value.norm() < 1e-9) {
return value.times(0);
}
return value.unit().times(copySignPow(value.norm(), exponent, maxMagnitude));
}
/**
* Raises the norm of the input to the power of the given exponent while preserving its direction.
*
* @param value The input vector to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be
* positive.
* @param <R> The number of rows in the vector.
* @return The transformed value with the same direction.
*/
public static <R extends Num> Vector<R> copyDirectionPow(Vector<R> value, double exponent) {
return copyDirectionPow(value, exponent, 1);
}
/**
* Returns modulus of input.
*

76
wpimath/src/main/native/include/frc/MathUtil.h
View File

@@ -94,15 +94,41 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
}
}
/**
* Returns a zero vector if the given vector is within the specified
* distance from the origin. The remaining distance between the deadband and the
* maximum distance is scaled from the origin to the maximum distance.
*
* @param value Value to clip.
* @param deadband Distance from origin.
* @param maxMagnitude The maximum distance from the origin of the input
* (defaults to 1). Can be infinite.
* @return The value after the deadband is applied.
*/
template <typename T, int N>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
Eigen::Vector<T, N> ApplyDeadband(const Eigen::Vector<T, N>& value, T deadband,
T maxMagnitude = T{1.0}) {
if constexpr (std::is_arithmetic_v<T>) {
if (value.norm() < T{1e-9}) {
return Eigen::Vector<T, N>::Zero();
}
return value.normalized() *
ApplyDeadband(value.norm(), deadband, maxMagnitude);
} else {
const Eigen::Vector<double, N> asDouble = value.template cast<double>();
const Eigen::Vector<double, N> processed =
ApplyDeadband(asDouble, deadband.value(), maxMagnitude.value());
return processed.template cast<T>();
}
}
/**
* Raises the input to the power of the given exponent while preserving its
* sign.
*
* The function normalizes the input value to the range [0, 1] based on the
* maximum magnitude, raises it to the power of the exponent, then scales the
* result back to the original range and copying the sign. This keeps the value
* in the original range and gives consistent curve behavior regardless of the
* input value's scale.
* maximum magnitude so that the output stays in the range.
*
* This is useful for applying smoother or more aggressive control response
* curves (e.g. joystick input shaping).
@@ -110,8 +136,8 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
* @param value The input value to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
* curve). Must be positive.
* @param maxMagnitude The maximum expected absolute value of input. Must be
* positive.
* @param maxMagnitude The maximum expected absolute value of input (defaults to
* 1). Must be positive.
* @return The transformed value with the same sign and scaled to the input
* range.
*/
@@ -130,6 +156,42 @@ constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
}
}
/**
* Raises the norm of the input to the power of the given exponent while
* preserving its direction.
*
* The function normalizes the input value to the range [0, 1] based on the
* maximum magnitude so that the output stays in the range.
*
* This is useful for applying smoother or more aggressive control response
* curves (e.g. joystick input shaping).
*
* @param value The input vector to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
* curve). Must be positive.
* @param maxMagnitude The maximum expected distance from origin of input
* (defaults to 1). Must be positive.
* @return The transformed value with the same direction and norm scaled to
* the input range.
*/
template <typename T, int N>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
Eigen::Vector<T, N> CopyDirectionPow(const Eigen::Vector<T, N>& value,
double exponent, T maxMagnitude = T{1.0}) {
if constexpr (std::is_arithmetic_v<T>) {
if (value.norm() < T{1e-9}) {
return Eigen::Vector<T, N>::Zero();
}
return value.normalized() *
CopySignPow(value.norm(), exponent, maxMagnitude);
} else {
const Eigen::Vector<double, N> asDouble = value.template cast<double>();
const Eigen::Vector<double, N> processed =
CopyDirectionPow(asDouble, exponent, maxMagnitude.value());
return processed.template cast<T>();
}
}
/**
* Returns modulus of input.
*
@@ -279,6 +341,7 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
// NOLINTNEXTLINE(bugprone-integer-division)
return current + diff * (maxVelocity * dt / dist);
}
return next;
@@ -309,6 +372,7 @@ constexpr Translation3d SlewRateLimit(const Translation3d& current,
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
// NOLINTNEXTLINE(bugprone-integer-division)
return current + diff * (maxVelocity * dt / dist);
}
return next;

125
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -72,6 +72,60 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY));
}
@Test
void testApplyDeadband2dUnityScale() {
assertEquals(
VecBuilder.fill(0.0, 1.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 1.0), 0.02));
assertEquals(
VecBuilder.fill(0.0, -1.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, -1.0), 0.02));
assertEquals(
VecBuilder.fill(-1.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(-1.0, 0.0), 0.02));
// == 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02));
// > 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.01, 0.0), 0.02));
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.02, 0.0), 0.02));
assertEquals(
VecBuilder.fill((0.03 - 0.02) / (1.0 - 0.02), 0.0),
MathUtil.applyDeadband(VecBuilder.fill(0.03, 0.0), 0.02));
assertEquals(
VecBuilder.fill(1.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(1.0, 0.0), 0.02));
}
@Test
void testApplyDeadband2dArbitraryScale() {
assertEquals(
VecBuilder.fill(0.0, 2.5), MathUtil.applyDeadband(VecBuilder.fill(0.0, 2.5), 0.02, 2.5));
assertEquals(
VecBuilder.fill(0.0, -2.5), MathUtil.applyDeadband(VecBuilder.fill(0.0, -2.5), 0.02, 2.5));
assertEquals(
VecBuilder.fill(-2.5, 0.0), MathUtil.applyDeadband(VecBuilder.fill(-2.5, 0.0), 0.02, 2.5));
// == 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02, 2.5));
// > 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.01, 0.0), 0.02, 2.5));
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.02, 0.0), 0.02, 2.5));
assertEquals(
VecBuilder.fill(2.5, 0.0), MathUtil.applyDeadband(VecBuilder.fill(2.5, 0.0), 0.02, 2.5));
}
@Test
void testApplyDeadband2dLargeMaxMagnitude() {
assertEquals(
VecBuilder.fill(80.0, 0.0),
MathUtil.applyDeadband(VecBuilder.fill(100.0, 0.0), 20, Double.POSITIVE_INFINITY));
}
@Test
void testCopySignPow() {
assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0));
@@ -110,6 +164,77 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0));
}
@Test
void testCopyDirectionPow2d() {
assertEquals(
VecBuilder.fill(0.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 1.0));
assertEquals(
VecBuilder.fill(-0.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 1.0));
assertEquals(
VecBuilder.fill(0.25, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 2.0));
assertEquals(
VecBuilder.fill(-0.25, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 2.0));
assertEquals(
VecBuilder.fill(Math.sqrt(0.5), 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(0.5, 0.0), 0.5));
assertEquals(
VecBuilder.fill(-Math.sqrt(0.5), 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(-0.5, 0.0), 0.5));
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.0), 2.0));
assertEquals(
VecBuilder.fill(1.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(1.0, 0.0), 2.0));
assertEquals(
VecBuilder.fill(-1.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(-1.0, 0.0), 2.0));
assertEquals(
VecBuilder.fill(0.0, Math.pow(0.8, 0.3)),
MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.8), 0.3));
assertEquals(
VecBuilder.fill(0.0, -Math.pow(0.8, 0.3)),
MathUtil.copyDirectionPow(VecBuilder.fill(0.0, -0.8), 0.3));
}
@Test
void testCopyDirectionPow2dMaxDistance() {
assertEquals(
VecBuilder.fill(5.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 1.0, 10.0));
assertEquals(
VecBuilder.fill(-5.0, 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 1.0, 10.0));
assertEquals(
VecBuilder.fill(2.5, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 2.0, 10.0));
assertEquals(
VecBuilder.fill(-2.5, 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 2.0, 10.0));
assertEquals(
VecBuilder.fill(Math.sqrt(0.5) * 10, 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 0.5, 10.0));
assertEquals(
VecBuilder.fill(-Math.sqrt(0.5) * 10, 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 0.5, 10.0));
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 0.0), 2.0, 5.0));
assertEquals(
VecBuilder.fill(5.0, 0.0), MathUtil.copyDirectionPow(VecBuilder.fill(5.0, 0.0), 2.0, 5.0));
assertEquals(
VecBuilder.fill(-5.0, 0.0),
MathUtil.copyDirectionPow(VecBuilder.fill(-5.0, 0.0), 2.0, 5.0));
assertEquals(
VecBuilder.fill(0.0, Math.pow(0.8, 0.3) * 100),
MathUtil.copyDirectionPow(VecBuilder.fill(0.0, 80.0), 0.3, 100.0));
assertEquals(
VecBuilder.fill(0.0, -Math.pow(0.8, 0.3) * 100),
MathUtil.copyDirectionPow(VecBuilder.fill(0.0, -80.0), 0.3, 100.0));
}
@Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the

183
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -55,8 +55,8 @@ TEST(MathUtilTest, ApplyDeadbandArbitraryScale) {
TEST(MathUtilTest, ApplyDeadbandUnits) {
// < 0
EXPECT_DOUBLE_EQ(
-20, frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad).value());
EXPECT_UNITS_EQ(-20_rad,
frc::ApplyDeadband<units::radian_t>(-20_rad, 1_rad, 20_rad));
}
TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
@@ -65,6 +65,72 @@ TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
}
TEST(MathUtilTest, ApplyDeadband2dUnityScale) {
EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {1.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-1.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{-1.0}, {0.0}}, 0.02));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02));
// > 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{(0.03 - 0.02) / (1.0 - 0.02)}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.03}, {0.0}}, 0.02));
EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{1.0}, {0.0}}, 0.02));
}
TEST(MathUtilTest, ApplyDeadband2dArbitraryScale) {
EXPECT_EQ((Eigen::Vector2d{{0.0}, {2.5}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {2.5}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-2.5}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {-2.5}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{-2.5}, {0.0}}, 0.02, 2.5));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5));
// > 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.01}, {0.0}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.02}, {0.0}}, 0.02, 2.5));
EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{2.5}, {0.0}}, 0.02, 2.5));
}
TEST(MathUtilTest, ApplyDeadband2dLargeMaxMagnitude) {
EXPECT_EQ((Eigen::Vector2d{{80.0}, {0.0}}),
(frc::ApplyDeadband(Eigen::Vector2d{{100.0}, {0.0}}, 20.0,
std::numeric_limits<double>::infinity())));
}
TEST(MathUtilTest, ApplyDeadband2dUnits) {
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 2.5_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 2.5_mps},
0.02_mps, 2.5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps},
0.02_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}),
frc::ApplyDeadband(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps},
0.02_mps, 2.5_mps));
}
TEST(MathUtilTest, CopySignPow) {
EXPECT_DOUBLE_EQ(0.5, frc::CopySignPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, frc::CopySignPow(-0.5, 1.0));
@@ -104,20 +170,107 @@ TEST(MathUtilTest, CopySignPowWithMaxMagnitude) {
}
TEST(MathUtilTest, CopySignPowWithUnits) {
EXPECT_DOUBLE_EQ(
0, frc::CopySignPow<units::meters_per_second_t>(0_mps, 2.0).value());
EXPECT_DOUBLE_EQ(
1, frc::CopySignPow<units::meters_per_second_t>(1_mps, 2.0).value());
EXPECT_DOUBLE_EQ(
-1, frc::CopySignPow<units::meters_per_second_t>(-1_mps, 2.0).value());
EXPECT_UNITS_EQ(0_mps,
frc::CopySignPow<units::meters_per_second_t>(0_mps, 2.0));
EXPECT_UNITS_EQ(1_mps,
frc::CopySignPow<units::meters_per_second_t>(1_mps, 2.0));
EXPECT_UNITS_EQ(-1_mps,
frc::CopySignPow<units::meters_per_second_t>(-1_mps, 2.0));
EXPECT_DOUBLE_EQ(
0.5 * 0.5 * 10,
frc::CopySignPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps).value());
EXPECT_DOUBLE_EQ(
-0.5 * 0.5 * 10,
frc::CopySignPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps)
.value());
EXPECT_UNITS_EQ(
units::meters_per_second_t{0.5 * 0.5 * 10},
frc::CopySignPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps));
EXPECT_UNITS_EQ(
units::meters_per_second_t{-0.5 * 0.5 * 10},
frc::CopySignPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
}
TEST(MathUtilTest, CopyDirectionPow2d) {
EXPECT_EQ((Eigen::Vector2d{{0.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 1.0));
EXPECT_EQ((Eigen::Vector2d{{-0.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 1.0));
EXPECT_EQ((Eigen::Vector2d{{0.25}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{-0.25}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5)}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.5}, {0.0}}, 0.5));
EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5)}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-0.5}, {0.0}}, 0.5));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{1.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{1.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{-1.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-1.0}, {0.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {1.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {1.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-1.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-1.0}}, 2.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3)}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.8}}, 0.3));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3)}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-0.8}}, 0.3));
}
TEST(MathUtilTest, CopyDirectionPow2dMaxDistance) {
EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 1.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 1.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{2.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-2.5}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 10.0));
EXPECT_EQ((Eigen::Vector2d{{std::sqrt(0.5) * 10.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 0.5, 10.0));
EXPECT_EQ((Eigen::Vector2d{{-std::sqrt(0.5) * 10.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 0.5, 10.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{5.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{-5.0}, {0.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{-5.0}, {0.0}}, 2.0, 5.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {std::pow(0.8, 0.3) * 100.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {80.0}}, 0.3, 100.0));
EXPECT_EQ((Eigen::Vector2d{{0.0}, {-std::pow(0.8, 0.3) * 100.0}}),
frc::CopyDirectionPow(Eigen::Vector2d{{0.0}, {-80.0}}, 0.3, 100.0));
}
TEST(MathUtilTest, CopyDirectionPow2dUnits) {
EXPECT_EQ(
(Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{1_mps, 0_mps}, 2.0));
EXPECT_EQ(
(Eigen::Vector<units::meters_per_second_t, 2>{-1_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{-1_mps, 0_mps}, 2.0));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{0_mps, 0_mps}, 2.0,
5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{5_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{5_mps, 0_mps}, 2.0,
5_mps));
EXPECT_EQ((Eigen::Vector<units::meters_per_second_t, 2>{-5_mps, 0_mps}),
frc::CopyDirectionPow(
Eigen::Vector<units::meters_per_second_t, 2>{-5_mps, 0_mps},
2.0, 5_mps));
}
TEST(MathUtilTest, InputModulus) {