WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
Translation3d.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 <Eigen/Core>
8#include <wpi/SymbolExports.h>
9#include <wpi/json_fwd.h>
10
13#include "units/length.h"
14#include "units/math.h"
15
16namespace frc {
17
18/**
19 * Represents a translation in 3D space.
20 * This object can be used to represent a point or a vector.
21 *
22 * This assumes that you are using conventional mathematical axes. When the
23 * robot is at the origin facing in the positive X direction, forward is
24 * positive X, left is positive Y, and up is positive Z.
25 */
27 public:
28 /**
29 * Constructs a Translation3d with X, Y, and Z components equal to zero.
30 */
31 constexpr Translation3d() = default;
32
33 /**
34 * Constructs a Translation3d with the X, Y, and Z components equal to the
35 * provided values.
36 *
37 * @param x The x component of the translation.
38 * @param y The y component of the translation.
39 * @param z The z component of the translation.
40 */
41 constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
42 : m_x{x}, m_y{y}, m_z{z} {}
43
44 /**
45 * Constructs a Translation3d with the provided distance and angle. This is
46 * essentially converting from polar coordinates to Cartesian coordinates.
47 *
48 * @param distance The distance from the origin to the end of the translation.
49 * @param angle The angle between the x-axis and the translation vector.
50 */
51 constexpr Translation3d(units::meter_t distance, const Rotation3d& angle) {
52 auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
53 m_x = rectangular.X();
54 m_y = rectangular.Y();
55 m_z = rectangular.Z();
56 }
57
58 /**
59 * Constructs a Translation3d from a 3D translation vector. The values are
60 * assumed to be in meters.
61 *
62 * @param vector The translation vector.
63 */
64 constexpr explicit Translation3d(const Eigen::Vector3d& vector)
65 : m_x{units::meter_t{vector.x()}},
66 m_y{units::meter_t{vector.y()}},
67 m_z{units::meter_t{vector.z()}} {}
68
69 /**
70 * Constructs a 3D translation from a 2D translation in the X-Y plane.
71 *
72 * @param translation The 2D translation.
73 * @see Pose3d(Pose2d)
74 * @see Transform3d(Transform2d)
75 */
76 constexpr explicit Translation3d(const Translation2d& translation)
77 : Translation3d{translation.X(), translation.Y(), 0_m} {}
78
79 /**
80 * Calculates the distance between two translations in 3D space.
81 *
82 * The distance between translations is defined as
83 * √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
84 *
85 * @param other The translation to compute the distance to.
86 *
87 * @return The distance between the two translations.
88 */
89 constexpr units::meter_t Distance(const Translation3d& other) const {
90 return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
91 units::math::pow<2>(other.m_y - m_y) +
92 units::math::pow<2>(other.m_z - m_z));
93 }
94
95 /**
96 * Returns the X component of the translation.
97 *
98 * @return The Z component of the translation.
99 */
100 constexpr units::meter_t X() const { return m_x; }
101
102 /**
103 * Returns the Y component of the translation.
104 *
105 * @return The Y component of the translation.
106 */
107 constexpr units::meter_t Y() const { return m_y; }
108
109 /**
110 * Returns the Z component of the translation.
111 *
112 * @return The Z component of the translation.
113 */
114 constexpr units::meter_t Z() const { return m_z; }
115
116 /**
117 * Returns a 3D translation vector representation of this translation.
118 *
119 * @return A 3D translation vector representation of this translation.
120 */
121 constexpr Eigen::Vector3d ToVector() const {
122 return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
123 }
124
125 /**
126 * Returns the norm, or distance from the origin to the translation.
127 *
128 * @return The norm of the translation.
129 */
130 constexpr units::meter_t Norm() const {
131 return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
132 }
133
134 /**
135 * Applies a rotation to the translation in 3D space.
136 *
137 * For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
138 * around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
139 *
140 * @param other The rotation to rotate the translation by.
141 *
142 * @return The new rotated translation.
143 */
144 constexpr Translation3d RotateBy(const Rotation3d& other) const {
145 Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
146 auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
147 return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},
148 units::meter_t{qprime.Z()}};
149 }
150
151 /**
152 * Rotates this translation around another translation in 3D space.
153 *
154 * @param other The other translation to rotate around.
155 * @param rot The rotation to rotate the translation by.
156 * @return The new rotated translation.
157 */
159 const Rotation3d& rot) const {
160 return (*this - other).RotateBy(rot) + other;
161 }
162
163 /**
164 * Returns a Translation2d representing this Translation3d projected into the
165 * X-Y plane.
166 */
167 constexpr Translation2d ToTranslation2d() const {
168 return Translation2d{m_x, m_y};
169 }
170
171 /**
172 * Returns the sum of two translations in 3D space.
173 *
174 * For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
175 * Translation3d{3.0, 8.0, 11.0}.
176 *
177 * @param other The translation to add.
178 *
179 * @return The sum of the translations.
180 */
181 constexpr Translation3d operator+(const Translation3d& other) const {
182 return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
183 }
184
185 /**
186 * Returns the difference between two translations.
187 *
188 * For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
189 * Translation3d{4.0, 2.0, 0.0}.
190 *
191 * @param other The translation to subtract.
192 *
193 * @return The difference between the two translations.
194 */
195 constexpr Translation3d operator-(const Translation3d& other) const {
196 return operator+(-other);
197 }
198
199 /**
200 * Returns the inverse of the current translation. This is equivalent to
201 * negating all components of the translation.
202 *
203 * @return The inverse of the current translation.
204 */
205 constexpr Translation3d operator-() const { return {-m_x, -m_y, -m_z}; }
206
207 /**
208 * Returns the translation multiplied by a scalar.
209 *
210 * For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
211 * 9.0}.
212 *
213 * @param scalar The scalar to multiply by.
214 *
215 * @return The scaled translation.
216 */
217 constexpr Translation3d operator*(double scalar) const {
218 return {scalar * m_x, scalar * m_y, scalar * m_z};
219 }
220
221 /**
222 * Returns the translation divided by a scalar.
223 *
224 * For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
225 * 2.25}.
226 *
227 * @param scalar The scalar to divide by.
228 *
229 * @return The scaled translation.
230 */
231 constexpr Translation3d operator/(double scalar) const {
232 return operator*(1.0 / scalar);
233 }
234
235 /**
236 * Checks equality between this Translation3d and another object.
237 *
238 * @param other The other object.
239 * @return Whether the two objects are equal.
240 */
241 constexpr bool operator==(const Translation3d& other) const {
242 return units::math::abs(m_x - other.m_x) < 1E-9_m &&
243 units::math::abs(m_y - other.m_y) < 1E-9_m &&
244 units::math::abs(m_z - other.m_z) < 1E-9_m;
245 }
246
247 private:
248 units::meter_t m_x = 0_m;
249 units::meter_t m_y = 0_m;
250 units::meter_t m_z = 0_m;
251};
252
254void to_json(wpi::json& json, const Translation3d& state);
255
257void from_json(const wpi::json& json, Translation3d& state);
258
259} // namespace frc
260
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a quaternion.
Definition Quaternion.h:19
constexpr Quaternion Inverse() const
Returns the inverse of the quaternion.
Definition Quaternion.h:130
A rotation in a 3D coordinate frame represented by a quaternion.
Definition Rotation3d.h:29
Represents a translation in 2D space.
Definition Translation2d.h:29
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
constexpr Translation3d operator/(double scalar) const
Returns the translation divided by a scalar.
Definition Translation3d.h:231
constexpr units::meter_t Y() const
Returns the Y component of the translation.
Definition Translation3d.h:107
constexpr Translation3d RotateAround(const Translation3d &other, const Rotation3d &rot) const
Rotates this translation around another translation in 3D space.
Definition Translation3d.h:158
constexpr Translation3d(units::meter_t x, units::meter_t y, units::meter_t z)
Constructs a Translation3d with the X, Y, and Z components equal to the provided values.
Definition Translation3d.h:41
constexpr bool operator==(const Translation3d &other) const
Checks equality between this Translation3d and another object.
Definition Translation3d.h:241
constexpr Translation3d operator*(double scalar) const
Returns the translation multiplied by a scalar.
Definition Translation3d.h:217
constexpr units::meter_t X() const
Returns the X component of the translation.
Definition Translation3d.h:100
constexpr units::meter_t Z() const
Returns the Z component of the translation.
Definition Translation3d.h:114
constexpr Translation3d(const Eigen::Vector3d &vector)
Constructs a Translation3d from a 3D translation vector.
Definition Translation3d.h:64
constexpr Translation3d(const Translation2d &translation)
Constructs a 3D translation from a 2D translation in the X-Y plane.
Definition Translation3d.h:76
constexpr Translation3d operator-() const
Returns the inverse of the current translation.
Definition Translation3d.h:205
constexpr Translation3d operator+(const Translation3d &other) const
Returns the sum of two translations in 3D space.
Definition Translation3d.h:181
constexpr units::meter_t Norm() const
Returns the norm, or distance from the origin to the translation.
Definition Translation3d.h:130
constexpr Translation3d operator-(const Translation3d &other) const
Returns the difference between two translations.
Definition Translation3d.h:195
constexpr units::meter_t Distance(const Translation3d &other) const
Calculates the distance between two translations in 3D space.
Definition Translation3d.h:89
constexpr Translation2d ToTranslation2d() const
Returns a Translation2d representing this Translation3d projected into the X-Y plane.
Definition Translation3d.h:167
constexpr Translation3d()=default
Constructs a Translation3d with X, Y, and Z components equal to zero.
constexpr Translation3d(units::meter_t distance, const Rotation3d &angle)
Constructs a Translation3d with the provided distance and angle.
Definition Translation3d.h:51
constexpr Eigen::Vector3d ToVector() const
Returns a 3D translation vector representation of this translation.
Definition Translation3d.h:121
constexpr UnitType abs(const UnitType x) noexcept
Compute absolute value.
Definition math.h:726
constexpr auto sqrt(const UnitType &value) noexcept -> unit_t< square_root< typename units::traits::unit_t_traits< UnitType >::unit_type >, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the square root of value
Definition math.h:485
Definition CAN.h:11
WPILIB_DLLEXPORT void to_json(wpi::json &json, const Rotation3d &rotation)
WPILIB_DLLEXPORT void from_json(const wpi::json &json, Rotation3d &rotation)
constexpr auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition base.h:2810
Unit Conversion Library namespace.
Definition acceleration.h:33
Type representing an arbitrary unit.
Definition base.h:888