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 CoordinateSystem.convert() translation and rotation overloads (#4227)

Browse Source
This commit is contained in:
Tyler Veness
2022-05-18 20:41:15 -07:00
committed by GitHub
parent 3fada4e0b4
commit 0d9956273c
5 changed files with 190 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,8 +24,8 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
R.block<3, 1>(0, 2) = positiveZ.m_axis;
// Require that the change of basis matrix is special orthogonal. This is true
// if the axes used are orthogonal and normalized. The Axis class already
// normalizes itself, so we just need to check for orthogonality.
// if the axes used are orthogonal and normalized. The CoordinateAxis class
// already normalizes itself, so we just need to check for orthogonality.
if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
throw std::domain_error("Coordinate system isn't special orthogonal");
}
@@ -87,6 +87,18 @@ const CoordinateSystem& CoordinateSystem::NED() {
return instance;
}
Translation3d CoordinateSystem::Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return translation.RotateBy(from.m_rotation - to.m_rotation);
}
Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return rotation.RotateBy(from.m_rotation - to.m_rotation);
}
Pose3d CoordinateSystem::Convert(const Pose3d& pose,
const CoordinateSystem& from,
const CoordinateSystem& to) {

25
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -10,6 +10,7 @@
#include "frc/geometry/CoordinateAxis.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Translation3d.h"
namespace frc {
@@ -58,6 +59,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
*/
static const CoordinateSystem& NED();
/**
* Converts the given translation from one coordinate system to another.
*
* @param translation The translation to convert.
* @param from The coordinate system the translation starts in.
* @param to The coordinate system to which to convert.
* @return The given translation in the desired coordinate system.
*/
static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to);
/**
* Converts the given rotation from one coordinate system to another.
*
* @param rotation The rotation to convert.
* @param from The coordinate system the rotation starts in.
* @param to The coordinate system to which to convert.
* @return The given rotation in the desired coordinate system.
*/
static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to);
/**
* Converts the given pose from one coordinate system to another.
*