WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
Transform3d.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <utility>
8
9#include <wpi/SymbolExports.h>
10
13
14namespace frc {
15
16class Pose3d;
17
18/**
19 * Represents a transformation for a Pose3d in the pose's frame.
20 */
22 public:
23 /**
24 * Constructs the transform that maps the initial pose to the final pose.
25 *
26 * @param initial The initial pose for the transformation.
27 * @param final The final pose for the transformation.
28 */
29 constexpr Transform3d(const Pose3d& initial, const Pose3d& final);
30
31 /**
32 * Constructs a transform with the given translation and rotation components.
33 *
34 * @param translation Translational component of the transform.
35 * @param rotation Rotational component of the transform.
36 */
37 constexpr Transform3d(Translation3d translation, Rotation3d rotation)
38 : m_translation{std::move(translation)},
39 m_rotation{std::move(rotation)} {}
40
41 /**
42 * Constructs a transform with x, y, and z translations instead of a separate
43 * Translation3d.
44 *
45 * @param x The x component of the translational component of the transform.
46 * @param y The y component of the translational component of the transform.
47 * @param z The z component of the translational component of the transform.
48 * @param rotation The rotational component of the transform.
49 */
50 constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
51 Rotation3d rotation)
52 : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
53
54 /**
55 * Constructs a transform with the specified affine transformation matrix.
56 *
57 * @param matrix The affine transformation matrix.
58 * @throws std::domain_error if the affine transformation matrix is invalid.
59 */
60 constexpr explicit Transform3d(const Eigen::Matrix4d& matrix)
61 : m_translation{Eigen::Vector3d{
62 {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
63 m_rotation{
64 Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
65 {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
66 {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
67 if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
68 matrix(3, 3) != 1.0) {
69 throw std::domain_error("Affine transformation matrix is invalid");
70 }
71 }
72
73 /**
74 * Constructs the identity transform -- maps an initial pose to itself.
75 */
76 constexpr Transform3d() = default;
77
78 /**
79 * Constructs a 3D transform from a 2D transform in the X-Y plane.
80 **
81 * @param transform The 2D transform.
82 * @see Rotation3d(Rotation2d)
83 * @see Translation3d(Translation2d)
84 */
85 constexpr explicit Transform3d(const frc::Transform2d& transform)
86 : m_translation{frc::Translation3d{transform.Translation()}},
87 m_rotation{frc::Rotation3d{transform.Rotation()}} {}
88
89 /**
90 * Returns the translation component of the transformation.
91 *
92 * @return Reference to the translational component of the transform.
93 */
94 constexpr const Translation3d& Translation() const { return m_translation; }
95
96 /**
97 * Returns the X component of the transformation's translation.
98 *
99 * @return The x component of the transformation's translation.
100 */
101 constexpr units::meter_t X() const { return m_translation.X(); }
102
103 /**
104 * Returns the Y component of the transformation's translation.
105 *
106 * @return The y component of the transformation's translation.
107 */
108 constexpr units::meter_t Y() const { return m_translation.Y(); }
109
110 /**
111 * Returns the Z component of the transformation's translation.
112 *
113 * @return The z component of the transformation's translation.
114 */
115 constexpr units::meter_t Z() const { return m_translation.Z(); }
116
117 /**
118 * Returns an affine transformation matrix representation of this
119 * transformation.
120 */
121 constexpr Eigen::Matrix4d ToMatrix() const {
122 auto vec = m_translation.ToVector();
123 auto mat = m_rotation.ToMatrix();
124 return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
125 {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
126 {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
127 {0.0, 0.0, 0.0, 1.0}};
128 }
129
130 /**
131 * Returns the rotational component of the transformation.
132 *
133 * @return Reference to the rotational component of the transform.
134 */
135 constexpr const Rotation3d& Rotation() const { return m_rotation; }
136
137 /**
138 * Invert the transformation. This is useful for undoing a transformation.
139 *
140 * @return The inverted transformation.
141 */
142 constexpr Transform3d Inverse() const {
143 // We are rotating the difference between the translations
144 // using a clockwise rotation matrix. This transforms the global
145 // delta into a local delta (relative to the initial pose).
146 return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
147 }
148
149 /**
150 * Multiplies the transform by the scalar.
151 *
152 * @param scalar The scalar.
153 * @return The scaled Transform3d.
154 */
155 constexpr Transform3d operator*(double scalar) const {
156 return Transform3d(m_translation * scalar, m_rotation * scalar);
157 }
158
159 /**
160 * Divides the transform by the scalar.
161 *
162 * @param scalar The scalar.
163 * @return The scaled Transform3d.
164 */
165 constexpr Transform3d operator/(double scalar) const {
166 return *this * (1.0 / scalar);
167 }
168
169 /**
170 * Composes two transformations. The second transform is applied relative to
171 * the orientation of the first.
172 *
173 * @param other The transform to compose with this one.
174 * @return The composition of the two transformations.
175 */
176 constexpr Transform3d operator+(const Transform3d& other) const;
177
178 /**
179 * Checks equality between this Transform3d and another object.
180 */
181 constexpr bool operator==(const Transform3d&) const = default;
182
183 private:
184 Translation3d m_translation;
185 Rotation3d m_rotation;
186};
187
188} // namespace frc
189
190#include "frc/geometry/Pose3d.h"
191
192namespace frc {
193
194constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
195 // We are rotating the difference between the translations
196 // using a clockwise rotation matrix. This transforms the global
197 // delta into a local delta (relative to the initial pose).
198 m_translation = (final.Translation() - initial.Translation())
199 .RotateBy(-initial.Rotation());
200
201 m_rotation = final.Rotation() - initial.Rotation();
202}
203
204constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {
205 return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
206}
207
208} // namespace frc
209
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Represents a 3D pose containing translational and rotational elements.
Definition Pose3d.h:28
constexpr Pose3d TransformBy(const Transform3d &other) const
Transforms the pose by the given transformation and returns the new transformed pose.
Definition Pose3d.h:328
constexpr const Translation3d & Translation() const
Returns the underlying translation.
Definition Pose3d.h:120
constexpr const Rotation3d & Rotation() const
Returns the underlying rotation.
Definition Pose3d.h:148
A rotation in a 3D coordinate frame represented by a quaternion.
Definition Rotation3d.h:29
Represents a transformation for a Pose2d in the pose's frame.
Definition Transform2d.h:21
Represents a transformation for a Pose3d in the pose's frame.
Definition Transform3d.h:21
constexpr const Rotation3d & Rotation() const
Returns the rotational component of the transformation.
Definition Transform3d.h:135
constexpr Eigen::Matrix4d ToMatrix() const
Returns an affine transformation matrix representation of this transformation.
Definition Transform3d.h:121
constexpr units::meter_t X() const
Returns the X component of the transformation's translation.
Definition Transform3d.h:101
constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation)
Constructs a transform with x, y, and z translations instead of a separate Translation3d.
Definition Transform3d.h:50
constexpr Transform3d operator*(double scalar) const
Multiplies the transform by the scalar.
Definition Transform3d.h:155
constexpr Transform3d operator+(const Transform3d &other) const
Composes two transformations.
Definition Transform3d.h:204
constexpr Transform3d(const Eigen::Matrix4d &matrix)
Constructs a transform with the specified affine transformation matrix.
Definition Transform3d.h:60
constexpr Transform3d()=default
Constructs the identity transform – maps an initial pose to itself.
constexpr Transform3d(Translation3d translation, Rotation3d rotation)
Constructs a transform with the given translation and rotation components.
Definition Transform3d.h:37
constexpr bool operator==(const Transform3d &) const =default
Checks equality between this Transform3d and another object.
constexpr units::meter_t Y() const
Returns the Y component of the transformation's translation.
Definition Transform3d.h:108
constexpr Transform3d Inverse() const
Invert the transformation.
Definition Transform3d.h:142
constexpr Transform3d operator/(double scalar) const
Divides the transform by the scalar.
Definition Transform3d.h:165
constexpr const Translation3d & Translation() const
Returns the translation component of the transformation.
Definition Transform3d.h:94
constexpr Transform3d(const frc::Transform2d &transform)
Constructs a 3D transform from a 2D transform in the X-Y plane.
Definition Transform3d.h:85
constexpr units::meter_t Z() const
Returns the Z component of the transformation's translation.
Definition Transform3d.h:115
Represents a translation in 3D space.
Definition Translation3d.h:26
constexpr Translation3d RotateBy(const Rotation3d &other) const
Applies a rotation to the translation in 3D space.
Definition Translation3d.h:144
Definition Variable.hpp:739
Definition CAN.h:11
Implement std::hash so that hash_code can be used in STL containers.
Definition PointerIntPair.h:280
Type representing an arbitrary unit.
Definition base.h:888