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 an operator for composing two Transform2ds (#3527)

Browse Source
This commit is contained in:
Tyler Veness
2021-08-23 20:22:48 -07:00
committed by GitHub
parent 2edd510ab7
commit e0c6cd3dcc
5 changed files with 60 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
View File

@@ -15,10 +15,10 @@ class Transform2dTest {
@Test
void testInverse() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
var transform = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
var transformed = initial.plus(transformation);
var untransformed = transformed.plus(transformation.inverse());
var transformed = initial.plus(transform);
var untransformed = transformed.plus(transform.inverse());
assertAll(
() -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
@@ -29,4 +29,23 @@ class Transform2dTest {
untransformed.getRotation().getDegrees(),
kEpsilon));
}
@Test
void testComposition() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transform1 = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
var transform2 = new Transform2d(new Translation2d(0.0, 2.0), Rotation2d.fromDegrees(5.0));
var transformedSeparate = initial.plus(transform1).plus(transform2);
var transformedCombined = initial.plus(transform1.plus(transform2));
assertAll(
() -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon),
() -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon),
() ->
assertEquals(
transformedSeparate.getRotation().getDegrees(),
transformedCombined.getRotation().getDegrees(),
kEpsilon));
}
}