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

[wpimath] Add RotateBy() function to pose classes (#5491)

Browse Source 
Fixes #5472.
This commit is contained in:
Tyler Veness
2023-07-31 21:16:44 -07:00
committed by GitHub
parent 26d6e68c8f
commit 35a8b129d9
10 changed files with 139 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -14,6 +14,28 @@ import org.junit.jupiter.api.Test;
class Pose2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testRotateBy() {
final double x = 1.0;
final double y = 2.0;
var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
var rotation = Rotation2d.fromDegrees(5.0);
var rotated = initial.rotateBy(rotation);
// Translation is rotated by CCW rotation matrix
double c = rotation.getCos();
double s = rotation.getSin();
assertAll(
() -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
() -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
() ->
assertEquals(
initial.getRotation().getDegrees() + rotation.getDegrees(),
rotated.getRotation().getDegrees(),
kEpsilon));
}
@Test
void testTransformBy() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));

32
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -17,6 +17,38 @@ import org.junit.jupiter.api.Test;
class Pose3dTest {
private static final double kEpsilon = 1E-9;
@Test
void testRotateBy() {
final double x = 1.0;
final double y = 2.0;
var initial =
new Pose3d(
new Translation3d(x, y, 0.0),
new Rotation3d(
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0),
Units.degreesToRadians(45.0)));
double yaw = Units.degreesToRadians(5.0);
var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
var rotated = initial.rotateBy(rotation);
// Translation is rotated by CCW rotation matrix
double c = Math.cos(yaw);
double s = Math.sin(yaw);
assertAll(
() -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
() -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
() -> assertEquals(0.0, rotated.getZ(), kEpsilon),
() -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
() -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
() ->
assertEquals(
initial.getRotation().getZ() + rotation.getZ(),
rotated.getRotation().getZ(),
kEpsilon));
}
@Test
void testTransformByRotations() {
var initialPose =