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] 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

6
wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp
View File

@@ -68,6 +68,8 @@ Pose3d CoordinateSystem::Convert(const Pose3d& pose,
Transform3d CoordinateSystem::Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return Transform3d{Convert(transform.Translation(), from, to),
Convert(transform.Rotation(), from, to)};
const auto coordRot = from.m_rotation - to.m_rotation;
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
}