WPILibC++ 2023.4.3
Rotation3d.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 <wpi/SymbolExports.h>
8
9#include "Quaternion.h"
10#include "Rotation2d.h"
11#include "frc/EigenCore.h"
12#include "units/angle.h"
13
14namespace wpi {
15class json;
16} // namespace wpi
17
18namespace frc {
19
20/**
21 * A rotation in a 3D coordinate frame represented by a quaternion.
22 */
24 public:
25 /**
26 * Constructs a Rotation3d with a default angle of 0 degrees.
27 */
28 Rotation3d() = default;
29
30 /**
31 * Constructs a Rotation3d from a quaternion.
32 *
33 * @param q The quaternion.
34 */
35 explicit Rotation3d(const Quaternion& q);
36
37 /**
38 * Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
39 *
40 * Extrinsic rotations occur in that order around the axes in the fixed global
41 * frame rather than the body frame.
42 *
43 * Angles are measured counterclockwise with the rotation axis pointing "out
44 * of the page". If you point your right thumb along the positive axis
45 * direction, your fingers curl in the direction of positive rotation.
46 *
47 * @param roll The counterclockwise rotation angle around the X axis (roll).
48 * @param pitch The counterclockwise rotation angle around the Y axis (pitch).
49 * @param yaw The counterclockwise rotation angle around the Z axis (yaw).
50 */
51 Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
52
53 /**
54 * Constructs a Rotation3d with the given axis-angle representation. The axis
55 * doesn't have to be normalized.
56 *
57 * @param axis The rotation axis.
58 * @param angle The rotation around the axis.
59 */
60 Rotation3d(const Vectord<3>& axis, units::radian_t angle);
61
62 /**
63 * Constructs a Rotation3d with the given rotation vector representation. This
64 * representation is equivalent to axis-angle, where the normalized axis is
65 * multiplied by the rotation around the axis in radians.
66 *
67 * @param rvec The rotation vector.
68 */
69 explicit Rotation3d(const Eigen::Vector3d& rvec);
70
71 /**
72 * Constructs a Rotation3d from a rotation matrix.
73 *
74 * @param rotationMatrix The rotation matrix.
75 * @throws std::domain_error if the rotation matrix isn't special orthogonal.
76 */
77 explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
78
79 /**
80 * Constructs a Rotation3d that rotates the initial vector onto the final
81 * vector.
82 *
83 * This is useful for turning a 3D vector (final) into an orientation relative
84 * to a coordinate system vector (initial).
85 *
86 * @param initial The initial vector.
87 * @param final The final vector.
88 */
89 Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
90
91 /**
92 * Adds two rotations together.
93 *
94 * @param other The rotation to add.
95 *
96 * @return The sum of the two rotations.
97 */
98 Rotation3d operator+(const Rotation3d& other) const;
99
100 /**
101 * Subtracts the new rotation from the current rotation and returns the new
102 * rotation.
103 *
104 * @param other The rotation to subtract.
105 *
106 * @return The difference between the two rotations.
107 */
108 Rotation3d operator-(const Rotation3d& other) const;
109
110 /**
111 * Takes the inverse of the current rotation.
112 *
113 * @return The inverse of the current rotation.
114 */
116
117 /**
118 * Multiplies the current rotation by a scalar.
119 *
120 * @param scalar The scalar.
121 *
122 * @return The new scaled Rotation3d.
123 */
125
126 /**
127 * Divides the current rotation by a scalar.
128 *
129 * @param scalar The scalar.
130 *
131 * @return The new scaled Rotation3d.
132 */
134
135 /**
136 * Checks equality between this Rotation3d and another object.
137 */
138 bool operator==(const Rotation3d&) const = default;
139
140 /**
141 * Adds the new rotation to the current rotation.
142 *
143 * @param other The rotation to rotate by.
144 *
145 * @return The new rotated Rotation3d.
146 */
147 Rotation3d RotateBy(const Rotation3d& other) const;
148
149 /**
150 * Returns the quaternion representation of the Rotation3d.
151 */
152 const Quaternion& GetQuaternion() const;
153
154 /**
155 * Returns the counterclockwise rotation angle around the X axis (roll).
156 */
157 units::radian_t X() const;
158
159 /**
160 * Returns the counterclockwise rotation angle around the Y axis (pitch).
161 */
162 units::radian_t Y() const;
163
164 /**
165 * Returns the counterclockwise rotation angle around the Z axis (yaw).
166 */
167 units::radian_t Z() const;
168
169 /**
170 * Returns the axis in the axis-angle representation of this rotation.
171 */
173
174 /**
175 * Returns the angle in the axis-angle representation of this rotation.
176 */
177 units::radian_t Angle() const;
178
179 /**
180 * Returns a Rotation2d representing this Rotation3d projected into the X-Y
181 * plane.
182 */
184
185 private:
186 Quaternion m_q;
187};
188
190void to_json(wpi::json& json, const Rotation3d& rotation);
191
193void from_json(const wpi::json& json, Rotation3d& rotation);
194
195} // namespace frc
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Definition: Quaternion.h:17
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:26
A rotation in a 3D coordinate frame represented by a quaternion.
Definition: Rotation3d.h:23
Rotation2d ToRotation2d() const
Returns a Rotation2d representing this Rotation3d projected into the X-Y plane.
Rotation3d operator-() const
Takes the inverse of the current rotation.
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw)
Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
Rotation3d operator+(const Rotation3d &other) const
Adds two rotations together.
Rotation3d operator-(const Rotation3d &other) const
Subtracts the new rotation from the current rotation and returns the new rotation.
units::radian_t Y() const
Returns the counterclockwise rotation angle around the Y axis (pitch).
Rotation3d(const Eigen::Vector3d &rvec)
Constructs a Rotation3d with the given rotation vector representation.
Rotation3d operator/(double scalar) const
Divides the current rotation by a scalar.
units::radian_t X() const
Returns the counterclockwise rotation angle around the X axis (roll).
units::radian_t Z() const
Returns the counterclockwise rotation angle around the Z axis (yaw).
Rotation3d(const Eigen::Matrix3d &rotationMatrix)
Constructs a Rotation3d from a rotation matrix.
units::radian_t Angle() const
Returns the angle in the axis-angle representation of this rotation.
Rotation3d(const Vectord< 3 > &axis, units::radian_t angle)
Constructs a Rotation3d with the given axis-angle representation.
Rotation3d RotateBy(const Rotation3d &other) const
Adds the new rotation to the current rotation.
Rotation3d operator*(double scalar) const
Multiplies the current rotation by a scalar.
Vectord< 3 > Axis() const
Returns the axis in the axis-angle representation of this rotation.
Rotation3d(const Quaternion &q)
Constructs a Rotation3d from a quaternion.
const Quaternion & GetQuaternion() const
Returns the quaternion representation of the Rotation3d.
Rotation3d(const Vectord< 3 > &initial, const Vectord< 3 > &final)
Constructs a Rotation3d that rotates the initial vector onto the final vector.
Rotation3d()=default
Constructs a Rotation3d with a default angle of 0 degrees.
bool operator==(const Rotation3d &) const =default
Checks equality between this Rotation3d and another object.
a class to store JSON values
Definition: json.h:2655
Definition: AprilTagFieldLayout.h:22
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
WPILIB_DLLEXPORT void from_json(const wpi::json &json, AprilTagFieldLayout &layout)
WPILIB_DLLEXPORT void to_json(wpi::json &json, const AprilTagFieldLayout &layout)
unit< std::ratio< 1 >, units::category::scalar_unit > scalar
Definition: base.h:2510
/file This file defines the SmallVector class.
Definition: AprilTagFieldLayout.h:18