34 constexpr Quaternion(
double w,
double x,
double y,
double z)
35 : m_w{w}, m_x{x}, m_y{y}, m_z{z} {}
43 return Quaternion{m_w + other.m_w, m_x + other.m_x, m_y + other.m_y,
53 return Quaternion{m_w - other.m_w, m_x - other.m_x, m_y - other.m_y,
63 return Quaternion{m_w * other, m_x * other, m_y * other, m_z * other};
72 return Quaternion{m_w / other, m_x / other, m_y / other, m_z / other};
83 const auto& r2 = other.m_w;
86 double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
89 double cross_x = m_y * other.m_z - other.m_y * m_z;
90 double cross_y = other.m_x * m_z - m_x * other.m_z;
91 double cross_z = m_x * other.m_y - other.m_x * m_y;
96 r1 * other.m_x + r2 * m_x + cross_x,
97 r1 * other.m_y + r2 * m_y + cross_y,
98 r1 * other.m_z + r2 * m_z + cross_z};
108 return gcem::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
116 return W() * other.W() + X() * other.X() + Y() * other.Y() +
131 double norm = Norm();
132 return Conjugate() / (norm * norm);
139 double norm = Norm();
165 return other.
Exp() * *
this;
179 double axial_magnitude =
gcem::hypot(m_x, m_y, m_z);
180 double cosine =
gcem::cos(axial_magnitude);
184 if (axial_magnitude < 1e-9) {
186 double axial_magnitude_sq = axial_magnitude * axial_magnitude;
187 double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
189 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
191 axial_scalar =
gcem::sin(axial_magnitude) / axial_magnitude;
194 return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
195 Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
204 return (other * Inverse()).
Log();
216 double norm = Norm();
221 double s_norm = W() / norm;
224 return Quaternion{scalar, -std::numbers::pi, 0, 0};
233 v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
238 return Quaternion{scalar, v_scalar * m_x, v_scalar * m_y, v_scalar * m_z};
244 constexpr double W()
const {
return m_w; }
249 constexpr double X()
const {
return m_x; }
254 constexpr double Y()
const {
return m_y; }
259 constexpr double Z()
const {
return m_z; }
276 scalar = (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W()));
285 return Eigen::Vector3d{{scalar * m_x, scalar * m_y, scalar * m_z}};
302 double theta =
gcem::hypot(rvec(0), rvec(1), rvec(2));
310 axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
312 axial_scalar =
gcem::sin(theta / 2) / theta;
315 return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
316 axial_scalar * rvec(2)};
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
namespace for Niels Lohmann
Definition json.h:99
Represents a quaternion.
Definition Quaternion.h:19
constexpr Quaternion Log(const Quaternion &other) const
Log operator of a quaternion.
Definition Quaternion.h:203
constexpr double Dot(const Quaternion &other) const
Returns the elementwise product of two quaternions.
Definition Quaternion.h:115
constexpr bool operator==(const Quaternion &other) const
Checks equality between this Quaternion and another object.
Definition Quaternion.h:107
constexpr double Z() const
Returns Z component of the quaternion.
Definition Quaternion.h:259
constexpr Quaternion operator+(const Quaternion &other) const
Adds with another quaternion.
Definition Quaternion.h:42
constexpr Quaternion Normalize() const
Normalizes the quaternion.
Definition Quaternion.h:138
constexpr Quaternion operator*(const Quaternion &other) const
Multiply with another quaternion.
Definition Quaternion.h:80
constexpr Quaternion(double w, double x, double y, double z)
Constructs a quaternion with the given components.
Definition Quaternion.h:34
constexpr Quaternion operator*(double other) const
Multiples with a scalar value.
Definition Quaternion.h:62
constexpr Quaternion Conjugate() const
Returns the conjugate of the quaternion.
Definition Quaternion.h:123
constexpr Quaternion Pow(double t) const
Calculates this quaternion raised to a power.
Definition Quaternion.h:157
constexpr Quaternion operator/(double other) const
Divides by a scalar value.
Definition Quaternion.h:71
constexpr double Y() const
Returns Y component of the quaternion.
Definition Quaternion.h:254
constexpr Quaternion()=default
Constructs a quaternion with a default angle of 0 degrees.
constexpr Quaternion operator-(const Quaternion &other) const
Subtracts another quaternion.
Definition Quaternion.h:52
constexpr double Norm() const
Calculates the L2 norm of the quaternion.
Definition Quaternion.h:150
static constexpr Quaternion FromRotationVector(const Eigen::Vector3d &rvec)
Returns the quaternion representation of this rotation vector.
Definition Quaternion.h:295
constexpr Eigen::Vector3d ToRotationVector() const
Returns the rotation vector representation of this quaternion.
Definition Quaternion.h:266
constexpr double X() const
Returns X component of the quaternion.
Definition Quaternion.h:249
constexpr double W() const
Returns W component of the quaternion.
Definition Quaternion.h:244
constexpr Quaternion Exp() const
Matrix exponential of a quaternion.
Definition Quaternion.h:176
constexpr Quaternion Inverse() const
Returns the inverse of the quaternion.
Definition Quaternion.h:130
constexpr Quaternion Log() const
Log operator of a quaternion.
Definition Quaternion.h:215
constexpr Quaternion Exp(const Quaternion &other) const
Matrix exponential of a quaternion.
Definition Quaternion.h:164
WPILIB_DLLEXPORT void to_json(wpi::json &json, const Rotation3d &rotation)
WPILIB_DLLEXPORT void from_json(const wpi::json &json, Rotation3d &rotation)
constexpr T abs(const T x) noexcept
Compile-time absolute value function.
Definition abs.hpp:40
constexpr return_t< T > log(const T x) noexcept
Compile-time natural logarithm function.
Definition log.hpp:186
constexpr return_t< T > cos(const T x) noexcept
Compile-time cosine function.
Definition cos.hpp:83
constexpr return_t< T > sin(const T x) noexcept
Compile-time sine function.
Definition sin.hpp:85
constexpr common_return_t< T1, T2 > hypot(const T1 x, const T2 y) noexcept
Compile-time Pythagorean addition function.
Definition hypot.hpp:147
constexpr return_t< T > sqrt(const T x) noexcept
Compile-time square-root function.
Definition sqrt.hpp:109
constexpr return_t< T > exp(const T x) noexcept
Compile-time exponential function.
Definition exp.hpp:130
constexpr common_return_t< T1, T2 > atan2(const T1 y, const T2 x) noexcept
Compile-time two-argument arctangent function.
Definition atan2.hpp:88