Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-10-11 23:54:41 -07:00
parent b37e2d9343 9e85f3cf55
commit 7a2a982e66
14 changed files with 546 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -89,41 +89,166 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
}
@Test
void testCopySignPow() {
assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0));
assertEquals(-0.5, MathUtil.copySignPow(-0.5, 1.0));
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));
assertEquals(0.5 * 0.5, MathUtil.copySignPow(0.5, 2.0));
assertEquals(-(0.5 * 0.5), MathUtil.copySignPow(-0.5, 2.0));
// == 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02));
assertEquals(Math.sqrt(0.5), MathUtil.copySignPow(0.5, 0.5));
assertEquals(-Math.sqrt(0.5), MathUtil.copySignPow(-0.5, 0.5));
assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0));
assertEquals(1.0, MathUtil.copySignPow(1.0, 2.0));
assertEquals(-1.0, MathUtil.copySignPow(-1.0, 2.0));
assertEquals(Math.pow(0.8, 0.3), MathUtil.copySignPow(0.8, 0.3));
assertEquals(-Math.pow(0.8, 0.3), MathUtil.copySignPow(-0.8, 0.3));
// > 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 testCopySignPowMaxMagnitude() {
assertEquals(5, MathUtil.copySignPow(5.0, 1.0, 10.0));
assertEquals(-5, MathUtil.copySignPow(-5.0, 1.0, 10.0));
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));
assertEquals(0.5 * 0.5 * 10, MathUtil.copySignPow(5.0, 2.0, 10.0));
assertEquals(-0.5 * 0.5 * 10, MathUtil.copySignPow(-5.0, 2.0, 10.0));
// == 0
assertEquals(
VecBuilder.fill(0.0, 0.0), MathUtil.applyDeadband(VecBuilder.fill(0.0, 0.0), 0.02, 2.5));
assertEquals(Math.sqrt(0.5) * 10, MathUtil.copySignPow(5.0, 0.5, 10.0));
assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copySignPow(-5.0, 0.5, 10.0));
// > 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));
}
assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0, 5.0));
assertEquals(5.0, MathUtil.copySignPow(5.0, 2.0, 5.0));
assertEquals(-5.0, MathUtil.copySignPow(-5.0, 2.0, 5.0));
@Test
void testApplyDeadband2dLargeMaxMagnitude() {
assertEquals(
VecBuilder.fill(80.0, 0.0),
MathUtil.applyDeadband(VecBuilder.fill(100.0, 0.0), 20, Double.POSITIVE_INFINITY));
}
assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(80, 0.3, 100.0));
assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0));
@Test
void testCopyDirectionPow() {
assertEquals(0.5, MathUtil.copyDirectionPow(0.5, 1.0));
assertEquals(-0.5, MathUtil.copyDirectionPow(-0.5, 1.0));
assertEquals(0.5 * 0.5, MathUtil.copyDirectionPow(0.5, 2.0));
assertEquals(-(0.5 * 0.5), MathUtil.copyDirectionPow(-0.5, 2.0));
assertEquals(Math.sqrt(0.5), MathUtil.copyDirectionPow(0.5, 0.5));
assertEquals(-Math.sqrt(0.5), MathUtil.copyDirectionPow(-0.5, 0.5));
assertEquals(0.0, MathUtil.copyDirectionPow(0.0, 2.0));
assertEquals(1.0, MathUtil.copyDirectionPow(1.0, 2.0));
assertEquals(-1.0, MathUtil.copyDirectionPow(-1.0, 2.0));
assertEquals(Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(0.8, 0.3));
assertEquals(-Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(-0.8, 0.3));
}
@Test
void testCopyDirectionPowMaxMagnitude() {
assertEquals(5, MathUtil.copyDirectionPow(5.0, 1.0, 10.0));
assertEquals(-5, MathUtil.copyDirectionPow(-5.0, 1.0, 10.0));
assertEquals(0.5 * 0.5 * 10, MathUtil.copyDirectionPow(5.0, 2.0, 10.0));
assertEquals(-0.5 * 0.5 * 10, MathUtil.copyDirectionPow(-5.0, 2.0, 10.0));
assertEquals(Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(5.0, 0.5, 10.0));
assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(-5.0, 0.5, 10.0));
assertEquals(0.0, MathUtil.copyDirectionPow(0.0, 2.0, 5.0));
assertEquals(5.0, MathUtil.copyDirectionPow(5.0, 2.0, 5.0));
assertEquals(-5.0, MathUtil.copyDirectionPow(-5.0, 2.0, 5.0));
assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(80, 0.3, 100.0));
assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(-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

238
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,59 +65,213 @@ TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
}
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));
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));
EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopySignPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopySignPow(-0.5, 2.0));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02));
EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopySignPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopySignPow(-0.5, 0.5));
EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, frc::CopySignPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, frc::CopySignPow(-1.0, 2.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopySignPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopySignPow(-0.8, 0.3));
// > 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, CopySignPowWithMaxMagnitude) {
EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 1.0, 10.0));
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));
EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopySignPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopySignPow(-5.0, 2.0, 10.0));
// == 0
EXPECT_EQ((Eigen::Vector2d{{0.0}, {0.0}}),
frc::ApplyDeadband(Eigen::Vector2d{{0.0}, {0.0}}, 0.02, 2.5));
EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopySignPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10, frc::CopySignPow(-5.0, 0.5, 10.0));
// > 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));
}
EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 2.0, 5.0));
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, CopyDirectionPow) {
EXPECT_DOUBLE_EQ(0.5, frc::CopyDirectionPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, frc::CopyDirectionPow(-0.5, 1.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopyDirectionPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopyDirectionPow(-0.5, 2.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopyDirectionPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopyDirectionPow(-0.5, 0.5));
EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, frc::CopyDirectionPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, frc::CopyDirectionPow(-1.0, 2.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopyDirectionPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopyDirectionPow(-0.8, 0.3));
}
TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) {
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopyDirectionPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopyDirectionPow(-5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopyDirectionPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10,
frc::CopyDirectionPow(-5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(0.0, frc::CopyDirectionPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100,
frc::CopySignPow(80.0, 0.3, 100.0));
frc::CopyDirectionPow(80.0, 0.3, 100.0));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100,
frc::CopySignPow(-80.0, 0.3, 100.0));
frc::CopyDirectionPow(-80.0, 0.3, 100.0));
}
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());
TEST(MathUtilTest, CopyDirectionPowWithUnits) {
EXPECT_UNITS_EQ(
0_mps, frc::CopyDirectionPow<units::meters_per_second_t>(0_mps, 2.0));
EXPECT_UNITS_EQ(
1_mps, frc::CopyDirectionPow<units::meters_per_second_t>(1_mps, 2.0));
EXPECT_UNITS_EQ(
-1_mps, frc::CopyDirectionPow<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::CopyDirectionPow<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::CopyDirectionPow<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) {