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

[wpimath] Fix CoordinateSystem.convert() Transform3d overload (#5532)

Browse Source
This commit is contained in:
amquake
2023-08-12 15:20:22 -07:00
committed by GitHub
parent b542e01a0b
commit 8121566258
4 changed files with 23 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java
View File

@@ -124,7 +124,9 @@ public class CoordinateSystem {
*/
public static Transform3d convert(
Transform3d transform, CoordinateSystem from, CoordinateSystem to) {
var coordRot = from.m_rotation.minus(to.m_rotation);
return new Transform3d(
convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to));
convert(transform.getTranslation(), from, to),
coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot)));
}
}