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

[wpimath] Add affine transformation constructors and getters to geometry API (#7430)

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 15:49:17 -08:00
committed by GitHub
parent f772bb141d
commit e222efaa01
32 changed files with 615 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <stdexcept>
#include <type_traits>
#include <utility>
@@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d {
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with the specified affine transformation matrix.
*
* @param matrix The affine transformation matrix.
* @throws std::domain_error if the affine transformation matrix is invalid.
*/
constexpr explicit Pose3d(const Eigen::Matrix4d& matrix)
: m_translation{Eigen::Vector3d{
{matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
m_rotation{
Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
{matrix(1, 0), matrix(1, 1), matrix(1, 2)},
{matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
matrix(3, 3) != 1.0) {
throw std::domain_error("Affine transformation matrix is invalid");
}
}
/**
* Constructs a 3D pose from a 2D pose in the X-Y plane.
*
@@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d {
*/
constexpr Twist3d Log(const Pose3d& end) const;
/**
* Returns an affine transformation matrix representation of this pose.
*/
constexpr Eigen::Matrix4d ToMatrix() const {
auto vec = m_translation.ToVector();
auto mat = m_rotation.ToMatrix();
return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
{mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
{mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
{0.0, 0.0, 0.0, 1.0}};
}
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/