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

[wpimath] Implement Translation3d.RotateAround (#7661)

Browse Source
This commit is contained in:
HarryXChen
2025-01-09 23:31:34 -05:00
committed by GitHub
parent 995bc98ccf
commit e37c35746a
5 changed files with 94 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
View File

@@ -78,6 +78,40 @@ class Translation3dTest {
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testRotateAround() {
var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
var translation = new Translation3d(1.0, 2.0, 3.0);
var around = new Translation3d(3.0, 2.0, 1.0);
var rotated1 =
translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(1.0, rotated1.getX(), kEpsilon),
() -> assertEquals(0.0, rotated1.getY(), kEpsilon),
() -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
var rotated2 =
translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(5.0, rotated2.getX(), kEpsilon),
() -> assertEquals(2.0, rotated2.getY(), kEpsilon),
() -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
var rotated3 =
translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(3.0, rotated3.getX(), kEpsilon),
() -> assertEquals(0.0, rotated3.getY(), kEpsilon),
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testToTranslation2d() {
var translation = new Translation3d(1.0, 2.0, 3.0);

11
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) {
const auto rotated = another.RotateBy(90_deg);
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, RotateAround) {
const Translation2d translation{2_m, 1_m};
const Translation2d other{3_m, 2_m};
const auto rotated = translation.RotateAround(other, 180_deg);
EXPECT_NEAR(4.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, Multiplication) {

27
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) {
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, RotateAround) {
Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Translation3d translation{1_m, 2_m, 3_m};
const Translation3d around{3_m, 2_m, 1_m};
const auto rotated1 =
translation.RotateAround(around, Rotation3d{xAxis, 90_deg});
EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon);
const auto rotated2 =
translation.RotateAround(around, Rotation3d{yAxis, 90_deg});
EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon);
const auto rotated3 =
translation.RotateAround(around, Rotation3d{zAxis, 90_deg});
EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, ToTranslation2d) {
Translation3d translation{1_m, 2_m, 3_m};
Translation2d expected{1_m, 2_m};