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

[wpimath] Add Pose2d and Pose3d rotateAround() (#7659)

Browse Source
This commit is contained in:
ハイドラント
2025-01-13 13:55:26 -06:00
committed by GitHub
parent fc9e413ce1
commit cd92b07321
8 changed files with 98 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -238,6 +238,17 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 2D space.
*
* @param point The point in 2D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -271,6 +271,17 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 3D space.
*
* @param point The point in 3D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*