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

[wpimath] Make geometry classes constexpr (#7222)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-18 16:08:41 -07:00
committed by GitHub
parent 2054d0f57e
commit 95b9bd880b
30 changed files with 1324 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

372
wpimath/src/main/native/include/frc/ct_matrix.h Normal file
View File

@@ -0,0 +1,372 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <concepts>
#include <initializer_list>
#include <type_traits>
#include <Eigen/Core>
#include <gcem.hpp>
namespace frc {
template <typename T>
concept EigenMatrixLike = std::derived_from<T, Eigen::MatrixBase<T>>;
/**
* Compile-time wrapper for Eigen::Matrix.
*
* @tparam Rows Rows of matrix.
* @tparam Cols Columns of matrix.
*/
template <typename Scalar, int Rows, int Cols>
class ct_matrix {
public:
constexpr ct_matrix() = default;
/**
* Constructs a scalar VariableMatrix from a nested list of Variables.
*
* @param list The nested list of Variables.
*/
constexpr ct_matrix( // NOLINT (runtime/explicit)
std::initializer_list<std::initializer_list<Scalar>> list)
: m_storage{list} {}
template <typename Derived>
requires std::derived_from<Derived, Eigen::MatrixBase<Derived>>
// NOLINTNEXTLINE (runtime/explicit)
constexpr ct_matrix(const Derived& mat) : m_storage{mat} {}
/**
* Returns number of rows.
*
* @return Number of rows.
*/
constexpr int rows() const { return m_storage.rows(); }
/**
* Returns number of columns.
*
* @return Number of columns.
*/
constexpr int cols() const { return m_storage.cols(); }
/**
* Returns reference to matrix element.
*
* @param row Row index.
* @param col Column index.
*/
constexpr const Scalar& operator()(int row, int col) const {
return m_storage.coeff(row, col);
}
/**
* Returns reference to matrix element.
*
* @param row Row index.
* @param col Column index.
*/
constexpr Scalar& operator()(int row, int col) {
return m_storage.coeffRef(row, col);
}
/**
* Returns reference to matrix element.
*
* @param index Index.
*/
constexpr const Scalar& operator()(int index) const
requires(Rows == 1 || Cols == 1)
{
return m_storage.coeff(index);
}
/**
* Returns reference to matrix element.
*
* @param index Index.
*/
constexpr Scalar& operator()(int index)
requires(Rows == 1 || Cols == 1)
{
return m_storage.coeffRef(index);
}
/**
* Constexpr version of Eigen's scalar multiplication operator.
*
* @param lhs LHS scalar.
* @param rhs RHS matrix.
* @return Result of multiplication.
*/
friend constexpr ct_matrix<Scalar, Rows, Cols> operator*(
Scalar lhs, const ct_matrix<Scalar, Rows, Cols>& rhs) {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int i = 0; i < rhs.rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
result(i, j) = lhs * rhs(i, j);
}
}
return result;
} else {
return lhs * rhs.m_storage;
}
}
/**
* Constexpr version of Eigen's matrix multiplication operator.
*
* @tparam Cols2 Columns of RHS matrix.
* @param lhs LHS matrix.
* @param rhs RHS matrix.
* @return Result of multiplication.
*/
template <int Cols2>
friend constexpr ct_matrix<Scalar, Rows, Cols2> operator*(
const ct_matrix<Scalar, Rows, Cols>& lhs,
const ct_matrix<Scalar, Rows, Cols2>& rhs) {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols2> result;
for (int i = 0; i < lhs.rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Scalar sum = 0.0;
for (int k = 0; k < lhs.cols(); ++k) {
sum += lhs(i, k) * rhs(k, j);
}
result(i, j) = sum;
}
}
return result;
} else {
return lhs.m_storage * rhs.storage();
}
}
/**
* Constexpr version of Eigen's matrix addition operator.
*
* @param lhs LHS matrix.
* @param rhs RHS matrix.
* @return Result of addition.
*/
friend constexpr ct_matrix<Scalar, Rows, Cols> operator+(
const ct_matrix<Scalar, Rows, Cols>& lhs,
const ct_matrix<Scalar, Rows, Cols>& rhs) {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
result(row, col) = lhs(row, col) + rhs(row, col);
}
}
return result;
} else {
return lhs.m_storage + rhs.m_storage;
}
}
/**
* Constexpr version of Eigen's matrix subtraction operator.
*
* @param lhs LHS matrix.
* @param rhs RHS matrix.
* @return Result of subtraction.
*/
friend constexpr ct_matrix<Scalar, Rows, Cols> operator-(
const ct_matrix<Scalar, Rows, Cols>& lhs,
const ct_matrix<Scalar, Rows, Cols>& rhs) {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
result(row, col) = lhs(row, col) - rhs(row, col);
}
}
return result;
} else {
return lhs.m_storage - rhs.m_storage;
}
}
/**
* Constexpr version of Eigen's transpose member function.
*
* @return Transpose of matrix.
*/
constexpr ct_matrix<Scalar, Cols, Rows> transpose() const {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Cols, Rows> result;
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
result(col, row) = (*this)(row, col);
}
}
return result;
} else {
return m_storage.transpose().eval();
}
}
/**
* Constexpr version of Eigen's identity function.
*
* @return Identity matrix of the specified size.
*/
static constexpr ct_matrix<Scalar, Rows, Cols> Identity()
requires(Rows != Eigen::Dynamic && Cols != Eigen::Dynamic)
{
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < Rows; ++row) {
for (int col = 0; col < Cols; ++col) {
if (row == col) {
result(row, row) = 1.0;
} else {
result(row, col) = 0.0;
}
}
}
return result;
} else {
return Eigen::Matrix<Scalar, Rows, Cols>::Identity();
}
}
/**
* Constexpr version of Eigen's vector dot member function.
*
* @tparam RhsRows Rows of RHS vector.
* @tparam RhsCols Columns of RHS vector.
* @param rhs RHS vector.
* @return Dot product of two vectors.
*/
template <int RhsRows, int RhsCols>
requires(Rows == 1 || Cols == 1) && (RhsRows == 1 || RhsCols == 1) &&
(Rows * Cols == RhsRows * RhsCols)
constexpr Scalar dot(const ct_matrix<Scalar, RhsRows, RhsCols>& rhs) const {
if (std::is_constant_evaluated()) {
Scalar sum = 0.0;
for (int index = 0; index < rows() * rhs.cols(); ++index) {
sum += (*this)(index)*rhs(index);
}
return sum;
} else {
return m_storage.dot(rhs.storage());
}
}
/**
* Constexpr version of Eigen's norm member function.
*
* @return Norm of matrix.
*/
constexpr Scalar norm() const {
if (std::is_constant_evaluated()) {
Scalar sum = 0.0;
for (int row = 0; row < Rows; ++row) {
for (int col = 0; col < Cols; ++col) {
sum += (*this)(row, col) * (*this)(row, col);
}
}
return gcem::sqrt(sum);
} else {
return m_storage.norm();
}
}
/**
* Constexpr version of Eigen's 3D vector cross member function.
*
* @param rhs RHS vector.
* @return Cross product of two vectors.
*/
constexpr ct_matrix<Scalar, 3, 1> cross(const ct_matrix<Scalar, 3, 1>& rhs)
requires(Rows == 3 && Cols == 1)
{
return Eigen::Vector3d{{(*this)(1) * rhs(2) - rhs(1) * (*this)(2),
rhs(0) * (*this)(2) - (*this)(0) * rhs(2),
(*this)(0) * rhs(1) - rhs(0) * (*this)(1)}};
}
/**
* Constexpr version of Eigen's 3x3 matrix determinant member function.
*
* @return Determinant of matrix.
*/
constexpr Scalar determinant() const
requires(Rows == 3 && Cols == 3)
{
// |a b c|
// |d e f| = aei + bfg + cgh - ceg - bdi - afh
// |g h i|
Scalar a = (*this)(0, 0);
Scalar b = (*this)(0, 1);
Scalar c = (*this)(0, 2);
Scalar d = (*this)(1, 0);
Scalar e = (*this)(1, 1);
Scalar f = (*this)(1, 2);
Scalar g = (*this)(2, 0);
Scalar h = (*this)(2, 1);
Scalar i = (*this)(2, 2);
return a * e * i + b * f * g + c * g * h - c * e * g - b * d * i -
a * f * h;
}
/**
* Returns the internal Eigen matrix.
*
* @return The internal Eigen matrix.
*/
constexpr const Eigen::Matrix<Scalar, Rows, Cols>& storage() const {
return m_storage;
}
/**
* Implicit cast to an Eigen matrix.
*/
constexpr operator Eigen::Matrix<Scalar, Rows, Cols>() const { // NOLINT
return m_storage;
}
private:
Eigen::Matrix<Scalar, Rows, Cols> m_storage;
};
template <typename Derived>
requires std::derived_from<Derived, Eigen::MatrixBase<Derived>>
ct_matrix(const Derived&)
-> ct_matrix<typename Derived::Scalar, Derived::RowsAtCompileTime,
Derived::ColsAtCompileTime>;
template <typename Scalar, int Rows>
using ct_vector = ct_matrix<Scalar, Rows, 1>;
template <typename Scalar, int Cols>
using ct_row_vector = ct_matrix<Scalar, 1, Cols>;
using ct_matrix3d = ct_matrix<double, 3, 3>;
using ct_vector3d = ct_vector<double, 3>;
} // namespace frc

18
wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h
View File

@@ -5,6 +5,7 @@
#pragma once
#include <Eigen/Core>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose3d.h"
@@ -26,7 +27,10 @@ class WPILIB_DLLEXPORT CoordinateAxis {
* @param y The y component.
* @param z The z component.
*/
CoordinateAxis(double x, double y, double z);
constexpr CoordinateAxis(double x, double y, double z) {
double norm = gcem::hypot(x, y, z);
m_axis = Eigen::Vector3d{{x / norm, y / norm, z / norm}};
}
CoordinateAxis(const CoordinateAxis&) = default;
CoordinateAxis& operator=(const CoordinateAxis&) = default;
@@ -37,32 +41,32 @@ class WPILIB_DLLEXPORT CoordinateAxis {
/**
* Returns a coordinate axis corresponding to +X in the NWU coordinate system.
*/
static const CoordinateAxis& N();
static constexpr CoordinateAxis N() { return CoordinateAxis{1.0, 0.0, 0.0}; }
/**
* Returns a coordinate axis corresponding to -X in the NWU coordinate system.
*/
static const CoordinateAxis& S();
static constexpr CoordinateAxis S() { return CoordinateAxis{-1.0, 0.0, 0.0}; }
/**
* Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
*/
static const CoordinateAxis& E();
static constexpr CoordinateAxis E() { return CoordinateAxis{0.0, -1.0, 0.0}; }
/**
* Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
*/
static const CoordinateAxis& W();
static constexpr CoordinateAxis W() { return CoordinateAxis{0.0, 1.0, 0.0}; }
/**
* Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
*/
static const CoordinateAxis& U();
static constexpr CoordinateAxis U() { return CoordinateAxis{0.0, 0.0, 1.0}; }
/**
* Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
*/
static const CoordinateAxis& D();
static constexpr CoordinateAxis D() { return CoordinateAxis{0.0, 0.0, -1.0}; }
private:
friend class CoordinateSystem;

74
wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h
View File

@@ -28,35 +28,54 @@ class WPILIB_DLLEXPORT CoordinateSystem {
* @param positiveZ The cardinal direction of the positive z-axis.
* @throws std::domain_error if the coordinate system isn't special orthogonal
*/
CoordinateSystem(const CoordinateAxis& positiveX,
const CoordinateAxis& positiveY,
const CoordinateAxis& positiveZ);
constexpr CoordinateSystem(const CoordinateAxis& positiveX,
const CoordinateAxis& positiveY,
const CoordinateAxis& positiveZ) {
// Construct a change of basis matrix from the source coordinate system to
// the NWU coordinate system. Each column vector in the change of basis
// matrix is one of the old basis vectors mapped to its representation in
// the new basis.
Eigen::Matrix3d R{{positiveX.m_axis.coeff(0), positiveY.m_axis.coeff(0),
positiveZ.m_axis.coeff(0)},
{positiveX.m_axis.coeff(1), positiveY.m_axis.coeff(1),
positiveZ.m_axis.coeff(1)},
{positiveX.m_axis.coeff(2), positiveY.m_axis.coeff(2),
positiveZ.m_axis.coeff(2)}};
CoordinateSystem(const CoordinateSystem&) = default;
CoordinateSystem& operator=(const CoordinateSystem&) = default;
CoordinateSystem(CoordinateSystem&&) = default;
CoordinateSystem& operator=(CoordinateSystem&&) = default;
// The change of basis matrix should be a pure rotation. The Rotation3d
// constructor will verify this by checking for special orthogonality.
m_rotation = Rotation3d{R};
}
/**
* Returns an instance of the North-West-Up (NWU) coordinate system.
*
* The +X axis is north, the +Y axis is west, and the +Z axis is up.
*/
static const CoordinateSystem& NWU();
constexpr static CoordinateSystem NWU() {
return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
CoordinateAxis::U()};
}
/**
* Returns an instance of the East-Down-North (EDN) coordinate system.
*
* The +X axis is east, the +Y axis is down, and the +Z axis is north.
*/
static const CoordinateSystem& EDN();
constexpr static CoordinateSystem EDN() {
return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
CoordinateAxis::N()};
}
/**
* Returns an instance of the NED coordinate system.
*
* The +X axis is north, the +Y axis is east, and the +Z axis is down.
*/
static const CoordinateSystem& NED();
constexpr static CoordinateSystem NED() {
return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
CoordinateAxis::D()};
}
/**
* Converts the given translation from one coordinate system to another.
@@ -66,9 +85,11 @@ class WPILIB_DLLEXPORT CoordinateSystem {
* @param to The coordinate system to which to convert.
* @return The given translation in the desired coordinate system.
*/
static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to);
constexpr static Translation3d Convert(const Translation3d& translation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return translation.RotateBy(from.m_rotation - to.m_rotation);
}
/**
* Converts the given rotation from one coordinate system to another.
@@ -78,9 +99,11 @@ class WPILIB_DLLEXPORT CoordinateSystem {
* @param to The coordinate system to which to convert.
* @return The given rotation in the desired coordinate system.
*/
static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to);
constexpr static Rotation3d Convert(const Rotation3d& rotation,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return rotation.RotateBy(from.m_rotation - to.m_rotation);
}
/**
* Converts the given pose from one coordinate system to another.
@@ -90,8 +113,12 @@ class WPILIB_DLLEXPORT CoordinateSystem {
* @param to The coordinate system to which to convert.
* @return The given pose in the desired coordinate system.
*/
static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
const CoordinateSystem& to);
constexpr static Pose3d Convert(const Pose3d& pose,
const CoordinateSystem& from,
const CoordinateSystem& to) {
return Pose3d{Convert(pose.Translation(), from, to),
Convert(pose.Rotation(), from, to)};
}
/**
* Converts the given transform from one coordinate system to another.
@@ -101,9 +128,14 @@ class WPILIB_DLLEXPORT CoordinateSystem {
* @param to The coordinate system to which to convert.
* @return The given transform in the desired coordinate system.
*/
static Transform3d Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to);
constexpr static Transform3d Convert(const Transform3d& transform,
const CoordinateSystem& from,
const CoordinateSystem& to) {
const auto coordRot = from.m_rotation - to.m_rotation;
return Transform3d{
Convert(transform.Translation(), from, to),
(-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))};
}
private:
// Rotation from this coordinate system to NWU coordinate system

4
wpimath/src/main/native/include/frc/geometry/Ellipse2d.h
View File

@@ -153,7 +153,9 @@ class WPILIB_DLLEXPORT Ellipse2d {
* @param point The point to check.
* @return The distance (0, if the point is contained by the ellipse)
*/
units::meter_t Distance(const Translation2d& point) const;
units::meter_t Distance(const Translation2d& point) const {
return FindNearestPoint(point).Distance(point);
}
/**
* Returns the nearest point that is contained within the ellipse.

122
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -4,21 +4,24 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
#include <utility>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "units/length.h"
namespace frc {
class Transform2d;
/**
* Represents a 2D pose containing translational and rotational elements.
*/
@@ -74,12 +77,12 @@ class WPILIB_DLLEXPORT Pose2d {
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform2d operator-(const Pose2d& other) const;
constexpr Transform2d operator-(const Pose2d& other) const;
/**
* Checks equality between this Pose2d and another object.
*/
bool operator==(const Pose2d&) const = default;
constexpr bool operator==(const Pose2d&) const = default;
/**
* Returns the underlying translation.
@@ -150,10 +153,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The transformed pose.
*/
constexpr Pose2d TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
constexpr Pose2d TransformBy(const Transform2d& other) const;
/**
* Returns the current pose relative to the given pose.
@@ -167,7 +167,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The current pose relative to the new origin pose.
*/
Pose2d RelativeTo(const Pose2d& other) const;
constexpr Pose2d RelativeTo(const Pose2d& other) const;
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
@@ -190,7 +190,7 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The new pose of the robot.
*/
Pose2d Exp(const Twist2d& twist) const;
constexpr Pose2d Exp(const Twist2d& twist) const;
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
@@ -200,21 +200,51 @@ class WPILIB_DLLEXPORT Pose2d {
*
* @return The twist that maps this to end.
*/
Twist2d Log(const Pose2d& end) const;
constexpr Twist2d Log(const Pose2d& end) const;
/**
* Returns the nearest Pose2d from a collection of poses
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
Pose2d Nearest(std::span<const Pose2d> poses) const;
constexpr Pose2d Nearest(std::span<const Pose2d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Radians().value()) <
gcem::abs(
(this->Rotation() - b.Rotation()).Radians().value());
}
return aDistance < bDistance;
});
}
/**
* Returns the nearest Pose2d from a collection of poses
* @param poses The collection of poses.
* @return The nearest Pose2d from the collection.
*/
Pose2d Nearest(std::initializer_list<Pose2d> poses) const;
constexpr Pose2d Nearest(std::initializer_list<Pose2d> poses) const {
return *std::min_element(
poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) {
auto aDistance = this->Translation().Distance(a.Translation());
auto bDistance = this->Translation().Distance(b.Translation());
// If the distances are equal sort by difference in rotation
if (aDistance == bDistance) {
return gcem::abs(
(this->Rotation() - a.Rotation()).Radians().value()) <
gcem::abs(
(this->Rotation() - b.Rotation()).Radians().value());
}
return aDistance < bDistance;
});
}
private:
Translation2d m_translation;
@@ -233,3 +263,71 @@ void from_json(const wpi::json& json, Pose2d& pose);
#include "frc/geometry/proto/Pose2dProto.h"
#endif
#include "frc/geometry/struct/Pose2dStruct.h"
#include "frc/geometry/Transform2d.h"
namespace frc {
constexpr Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d{pose.Translation(), pose.Rotation()};
}
constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
const Transform2d transform{other, *this};
return {transform.Translation(), transform.Rotation()};
}
constexpr Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
const auto dtheta = twist.dtheta.value();
const auto sinTheta = gcem::sin(dtheta);
const auto cosTheta = gcem::cos(dtheta);
double s, c;
if (gcem::abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
Rotation2d{cosTheta, sinTheta}};
return *this + transform;
}
constexpr Twist2d Pose2d::Log(const Pose2d& end) const {
const auto transform = end.RelativeTo(*this);
const auto dtheta = transform.Rotation().Radians().value();
const auto halfDtheta = dtheta / 2.0;
const auto cosMinusOne = transform.Rotation().Cos() - 1;
double halfThetaByTanOfHalfDtheta;
if (gcem::abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta =
-(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
}
const Translation2d translationPart =
transform.Translation().RotateBy(
{halfThetaByTanOfHalfDtheta, -halfDtheta}) *
gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
}
} // namespace frc

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

@@ -4,17 +4,23 @@
#pragma once
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "frc/ct_matrix.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation3d.h"
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Translation3d.h"
#include "frc/geometry/Twist3d.h"
namespace frc {
class Transform3d;
/**
* Represents a 3D pose containing translational and rotational elements.
*/
@@ -31,7 +37,9 @@ class WPILIB_DLLEXPORT Pose3d {
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(Translation3d translation, Rotation3d rotation);
constexpr Pose3d(Translation3d translation, Rotation3d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a pose with x, y, and z translations instead of a separate
@@ -42,15 +50,18 @@ class WPILIB_DLLEXPORT Pose3d {
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**
* Constructs a 3D pose from a 2D pose in the X-Y plane.
*
* @param pose The 2D pose.
*/
explicit Pose3d(const Pose2d& pose);
constexpr explicit Pose3d(const Pose2d& pose)
: m_translation{pose.X(), pose.Y(), 0_m},
m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
/**
* Transforms the pose by the given transformation and returns the new
@@ -62,7 +73,9 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The transformed pose.
*/
Pose3d operator+(const Transform3d& other) const;
constexpr Pose3d operator+(const Transform3d& other) const {
return TransformBy(other);
}
/**
* Returns the Transform3d that maps the one pose to another.
@@ -70,47 +83,47 @@ class WPILIB_DLLEXPORT Pose3d {
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform3d operator-(const Pose3d& other) const;
constexpr Transform3d operator-(const Pose3d& other) const;
/**
* Checks equality between this Pose3d and another object.
*/
bool operator==(const Pose3d&) const = default;
constexpr bool operator==(const Pose3d&) const = default;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation3d& Translation() const { return m_translation; }
constexpr const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
constexpr units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation3d& Rotation() const { return m_rotation; }
constexpr const Rotation3d& Rotation() const { return m_rotation; }
/**
* Multiplies the current pose by a scalar.
@@ -119,7 +132,9 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The new scaled Pose2d.
*/
Pose3d operator*(double scalar) const;
constexpr Pose3d operator*(double scalar) const {
return Pose3d{m_translation * scalar, m_rotation * scalar};
}
/**
* Divides the current pose by a scalar.
@@ -128,7 +143,9 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The new scaled Pose2d.
*/
Pose3d operator/(double scalar) const;
constexpr Pose3d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Rotates the pose around the origin and returns the new pose.
@@ -138,7 +155,9 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The rotated pose.
*/
Pose3d RotateBy(const Rotation3d& other) const;
constexpr Pose3d RotateBy(const Rotation3d& other) const {
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
}
/**
* Transforms the pose by the given transformation and returns the new
@@ -150,7 +169,7 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The transformed pose.
*/
Pose3d TransformBy(const Transform3d& other) const;
constexpr Pose3d TransformBy(const Transform3d& other) const;
/**
* Returns the current pose relative to the given pose.
@@ -164,7 +183,7 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The current pose relative to the new origin pose.
*/
Pose3d RelativeTo(const Pose3d& other) const;
constexpr Pose3d RelativeTo(const Pose3d& other) const;
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
@@ -185,7 +204,7 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The new pose of the robot.
*/
Pose3d Exp(const Twist3d& twist) const;
constexpr Pose3d Exp(const Twist3d& twist) const;
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output
@@ -195,12 +214,14 @@ class WPILIB_DLLEXPORT Pose3d {
*
* @return The twist that maps this to end.
*/
Twist3d Log(const Pose3d& end) const;
constexpr Twist3d Log(const Pose3d& end) const;
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/
Pose2d ToPose2d() const;
constexpr Pose2d ToPose2d() const {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}
private:
Translation3d m_translation;
@@ -219,3 +240,184 @@ void from_json(const wpi::json& json, Pose3d& pose);
#include "frc/geometry/proto/Pose3dProto.h"
#endif
#include "frc/geometry/struct/Pose3dStruct.h"
#include "frc/geometry/Transform3d.h"
namespace frc {
namespace detail {
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return ct_matrix3d{{0.0, -rotation(2), rotation(1)},
{rotation(2), 0.0, -rotation(0)},
{-rotation(1), rotation(0), 0.0}};
}
/**
* Applies the hat operator to a rotation vector.
*
* It takes a rotation vector and returns the corresponding matrix
* representation of the Lie algebra element (a 3x3 rotation matrix).
*
* @param rotation The rotation vector.
* @return The rotation vector as a 3x3 rotation matrix.
*/
constexpr Eigen::Matrix3d RotationVectorToMatrix(
const Eigen::Vector3d& rotation) {
// Given a rotation vector <a, b, c>,
// [ 0 -c b]
// Omega = [ c 0 -a]
// [-b a 0]
return Eigen::Matrix3d{{0.0, -rotation.coeff(2), rotation.coeff(1)},
{rotation.coeff(2), 0.0, -rotation.coeff(0)},
{-rotation.coeff(1), rotation.coeff(0), 0.0}};
}
} // namespace detail
constexpr Transform3d Pose3d::operator-(const Pose3d& other) const {
const auto pose = this->RelativeTo(other);
return Transform3d{pose.Translation(), pose.Rotation()};
}
constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
other.Rotation() + m_rotation};
}
constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
const Transform3d transform{other, *this};
return {transform.Translation(), transform.Rotation()};
}
constexpr Pose3d Pose3d::Exp(const Twist3d& twist) const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>(
const Twist3d& twist) -> Pose3d {
Vector3d u{{twist.dx.value(), twist.dy.value(), twist.dz.value()}};
Vector3d rvec{{twist.rx.value(), twist.ry.value(), twist.rz.value()}};
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double A;
double B;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/3! - θ²/5! + θ⁴/7!
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
// C:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120;
B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720;
C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A) / θ²
A = gcem::sin(theta) / theta;
B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A) / thetaSq;
}
Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq;
Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq;
Vector3d translation_component = V * u;
const Transform3d transform{
Translation3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)}},
Rotation3d{R}};
return *this + transform;
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>(twist);
} else {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(twist);
}
}
constexpr Twist3d Pose3d::Log(const Pose3d& end) const {
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
auto impl = [this]<typename Matrix3d, typename Vector3d>(
const Pose3d& end) -> Twist3d {
const auto transform = end.RelativeTo(*this);
Vector3d u{
{transform.X().value(), transform.Y().value(), transform.Z().value()}};
Vector3d rvec = transform.Rotation().GetQuaternion().ToRotationVector();
Matrix3d omega = detail::RotationVectorToMatrix(rvec);
Matrix3d omegaSq = omega * omega;
double theta = rvec.norm();
double thetaSq = theta * theta;
double C;
if (gcem::abs(theta) < 1E-7) {
// Taylor Expansions around θ = 0
// A = 1/1! - θ²/3! + θ⁴/5!
// B = 1/2! - θ²/4! + θ⁴/6!
// C = 1/6 * (1/2 + θ²/5! + θ⁴/7!)
// sources:
// A:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0
// B:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0
// C:
// https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0
C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240;
} else {
// A = std::sin(θ)/θ
// B = (1 - std::cos(θ)) / θ²
// C = (1 - A/(2*B)) / θ²
double A = gcem::sin(theta) / theta;
double B = (1 - gcem::cos(theta)) / thetaSq;
C = (1 - A / (2 * B)) / thetaSq;
}
Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
Vector3d translation_component = V_inv * u;
return Twist3d{units::meter_t{translation_component(0)},
units::meter_t{translation_component(1)},
units::meter_t{translation_component(2)},
units::radian_t{rvec(0)},
units::radian_t{rvec(1)},
units::radian_t{rvec(2)}};
};
if (std::is_constant_evaluated()) {
return impl.template operator()<ct_matrix3d, ct_vector3d>(end);
} else {
return impl.template operator()<Eigen::Matrix3d, Eigen::Vector3d>(end);
}
}
} // namespace frc

196
wpimath/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -4,7 +4,10 @@
#pragma once
#include <numbers>
#include <Eigen/Core>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -18,7 +21,7 @@ class WPILIB_DLLEXPORT Quaternion {
/**
* Constructs a quaternion with a default angle of 0 degrees.
*/
Quaternion() = default;
constexpr Quaternion() = default;
/**
* Constructs a quaternion with the given components.
@@ -28,42 +31,72 @@ class WPILIB_DLLEXPORT Quaternion {
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
Quaternion(double w, double x, double y, double z);
constexpr Quaternion(double w, double x, double y, double z)
: m_w{w}, m_x{x}, m_y{y}, m_z{z} {}
/**
* Adds with another quaternion.
*
* @param other the other quaternion
*/
Quaternion operator+(const Quaternion& other) const;
constexpr Quaternion operator+(const Quaternion& other) const {
return Quaternion{m_w + other.m_w, m_x + other.m_x, m_y + other.m_y,
m_z + other.m_z};
}
/**
* Subtracts another quaternion.
*
* @param other the other quaternion
*/
Quaternion operator-(const Quaternion& other) const;
constexpr Quaternion operator-(const Quaternion& other) const {
return Quaternion{m_w - other.m_w, m_x - other.m_x, m_y - other.m_y,
m_z - other.m_z};
}
/**
* Multiples with a scalar value.
*
* @param other the scalar value
*/
Quaternion operator*(const double other) const;
constexpr Quaternion operator*(double other) const {
return Quaternion{m_w * other, m_x * other, m_y * other, m_z * other};
}
/**
* Divides by a scalar value.
*
* @param other the scalar value
*/
Quaternion operator/(const double other) const;
constexpr Quaternion operator/(double other) const {
return Quaternion{m_w / other, m_x / other, m_y / other, m_z / other};
}
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
*/
Quaternion operator*(const Quaternion& other) const;
constexpr Quaternion operator*(const Quaternion& other) const {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
const auto& r1 = m_w;
const auto& r2 = other.m_w;
// v₁ ⋅ v₂
double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z;
// v₁ x v₂
double cross_x = m_y * other.m_z - other.m_y * m_z;
double cross_y = other.m_x * m_z - m_x * other.m_z;
double cross_z = m_x * other.m_y - other.m_x * m_y;
return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
r1 * r2 - dot,
// v = r₁v₂ + r₂v₁ + v₁ x v₂
r1 * other.m_x + r2 * m_x + cross_x,
r1 * other.m_y + r2 * m_y + cross_y,
r1 * other.m_z + r2 * m_z + cross_z};
}
/**
* Checks equality between this Quaternion and another object.
@@ -71,46 +104,66 @@ class WPILIB_DLLEXPORT Quaternion {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Quaternion& other) const;
constexpr bool operator==(const Quaternion& other) const {
return gcem::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
gcem::abs(Norm() - other.Norm()) < 1e-9;
}
/**
* Returns the elementwise product of two quaternions.
*/
double Dot(const Quaternion& other) const;
constexpr double Dot(const Quaternion& other) const {
return W() * other.W() + X() * other.X() + Y() * other.Y() +
Z() * other.Z();
}
/**
* Returns the conjugate of the quaternion.
*/
Quaternion Conjugate() const;
constexpr Quaternion Conjugate() const {
return Quaternion{W(), -X(), -Y(), -Z()};
}
/**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
constexpr Quaternion Inverse() const {
double norm = Norm();
return Conjugate() / (norm * norm);
}
/**
* Normalizes the quaternion.
*/
Quaternion Normalize() const;
constexpr Quaternion Normalize() const {
double norm = Norm();
if (norm == 0.0) {
return Quaternion{};
} else {
return Quaternion{W(), X(), Y(), Z()} / norm;
}
}
/**
* Calculates the L2 norm of the quaternion.
*/
double Norm() const;
constexpr double Norm() const { return gcem::sqrt(Dot(*this)); }
/**
* Calculates this quaternion raised to a power.
*
* @param t the power to raise this quaternion to.
*/
Quaternion Pow(const double t) const;
constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); }
/**
* Matrix exponential of a quaternion.
*
* @param other the "Twist" that will be applied to this quaternion.
*/
Quaternion Exp(const Quaternion& other) const;
constexpr Quaternion Exp(const Quaternion& other) const {
return other.Exp() * *this;
}
/**
* Matrix exponential of a quaternion.
@@ -120,14 +173,36 @@ class WPILIB_DLLEXPORT Quaternion {
* If this quaternion is in 𝖘𝖔(3) and you are looking for an element of
* SO(3), use FromRotationVector
*/
Quaternion Exp() const;
constexpr Quaternion Exp() const {
double scalar = gcem::exp(m_w);
double axial_magnitude = gcem::hypot(m_x, m_y, m_z);
double cosine = gcem::cos(axial_magnitude);
double axial_scalar;
if (axial_magnitude < 1e-9) {
// Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶)
double axial_magnitude_sq = axial_magnitude * axial_magnitude;
double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq;
axial_scalar =
1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0;
} else {
axial_scalar = gcem::sin(axial_magnitude) / axial_magnitude;
}
return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
}
/**
* Log operator of a quaternion.
*
* @param other The quaternion to map this quaternion onto
*/
Quaternion Log(const Quaternion& other) const;
constexpr Quaternion Log(const Quaternion& other) const {
return (other * Inverse()).Log();
}
/**
* Log operator of a quaternion.
@@ -137,34 +212,78 @@ class WPILIB_DLLEXPORT Quaternion {
* If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3),
* use ToRotationVector
*/
Quaternion Log() const;
constexpr Quaternion Log() const {
double norm = Norm();
double scalar = gcem::log(norm);
double v_norm = gcem::hypot(m_x, m_y, m_z);
double s_norm = W() / norm;
if (gcem::abs(s_norm + 1) < 1e-9) {
return Quaternion{scalar, -std::numbers::pi, 0, 0};
}
double v_scalar;
if (v_norm < 1e-9) {
// Taylor series expansion of atan2(y/x)/y at y = 0:
//
// 1/x - 1/3 y²/x³ + O(y⁴)
v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
} else {
v_scalar = gcem::atan2(v_norm, W()) / v_norm;
}
return Quaternion{scalar, v_scalar * m_x, v_scalar * m_y, v_scalar * m_z};
}
/**
* Returns W component of the quaternion.
*/
double W() const;
constexpr double W() const { return m_w; }
/**
* Returns X component of the quaternion.
*/
double X() const;
constexpr double X() const { return m_x; }
/**
* Returns Y component of the quaternion.
*/
double Y() const;
constexpr double Y() const { return m_y; }
/**
* Returns Z component of the quaternion.
*/
double Z() const;
constexpr double Z() const { return m_z; }
/**
* Returns the rotation vector representation of this quaternion.
*
* This is also the log operator of SO(3).
*/
Eigen::Vector3d ToRotationVector() const;
constexpr Eigen::Vector3d ToRotationVector() const {
// See equation (31) in "Integrating Generic Sensor Fusion Algorithms with
// Sound State Representation through Encapsulation of Manifolds"
//
// https://arxiv.org/pdf/1107.1119.pdf
double norm = gcem::hypot(m_x, m_y, m_z);
double scalar;
if (norm < 1e-9) {
scalar = (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W()));
} else {
if (W() < 0.0) {
scalar = 2.0 * gcem::atan2(-norm, -W()) / norm;
} else {
scalar = 2.0 * gcem::atan2(norm, W()) / norm;
}
}
return Eigen::Vector3d{{scalar * m_x, scalar * m_y, scalar * m_z}};
}
/**
* Returns the quaternion representation of this rotation vector.
@@ -173,14 +292,39 @@ class WPILIB_DLLEXPORT Quaternion {
*
* source: wpimath/algorithms.md
*/
static Quaternion FromRotationVector(const Eigen::Vector3d& rvec);
constexpr static Quaternion FromRotationVector(const Eigen::Vector3d& rvec) {
// 𝑣⃗ = θ * v̂
// v̂ = 𝑣⃗ / θ
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
double theta = gcem::hypot(rvec.coeff(0), rvec.coeff(1), rvec.coeff(2));
double cos = gcem::cos(theta / 2);
double axial_scalar;
if (theta < 1e-9) {
// taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 +
// O(θ⁴)
axial_scalar = 1.0 / 2.0 - theta * theta / 48.0;
} else {
axial_scalar = gcem::sin(theta / 2) / theta;
}
return Quaternion{cos, axial_scalar * rvec.coeff(0),
axial_scalar * rvec.coeff(1),
axial_scalar * rvec.coeff(2)};
}
private:
// Scalar r in versor form
double m_r = 1.0;
double m_w = 1.0;
// Vector v in versor form
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
double m_x = 0.0;
double m_y = 0.0;
double m_z = 0.0;
};
WPILIB_DLLEXPORT

259
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -4,13 +4,23 @@
#pragma once
#include <string>
#include <type_traits>
#include <Eigen/Core>
#include <Eigen/LU>
#include <fmt/format.h>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include "frc/ct_matrix.h"
#include "frc/fmt/Eigen.h"
#include "frc/geometry/Quaternion.h"
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "units/math.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -22,14 +32,14 @@ class WPILIB_DLLEXPORT Rotation3d {
/**
* Constructs a Rotation3d representing no rotation.
*/
Rotation3d() = default;
constexpr Rotation3d() = default;
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
explicit Rotation3d(const Quaternion& q);
constexpr explicit Rotation3d(const Quaternion& q) { m_q = q.Normalize(); }
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
@@ -45,7 +55,21 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
constexpr Rotation3d(units::radian_t roll, units::radian_t pitch,
units::radian_t yaw) {
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
double cr = units::math::cos(roll * 0.5);
double sr = units::math::sin(roll * 0.5);
double cp = units::math::cos(pitch * 0.5);
double sp = units::math::sin(pitch * 0.5);
double cy = units::math::cos(yaw * 0.5);
double sy = units::math::sin(yaw * 0.5);
m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy};
}
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis
@@ -54,7 +78,19 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle);
constexpr Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
double norm = ct_matrix{axis}.norm();
if (norm == 0.0) {
return;
}
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
Eigen::Vector3d v{{axis.coeff(0) / norm * units::math::sin(angle / 2.0),
axis.coeff(1) / norm * units::math::sin(angle / 2.0),
axis.coeff(2) / norm * units::math::sin(angle / 2.0)}};
m_q = Quaternion{units::math::cos(angle / 2.0), v.coeff(0), v.coeff(1),
v.coeff(2)};
}
/**
* Constructs a Rotation3d with the given rotation vector representation. This
@@ -63,7 +99,8 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @param rvec The rotation vector.
*/
explicit Rotation3d(const Eigen::Vector3d& rvec);
constexpr explicit Rotation3d(const Eigen::Vector3d& rvec)
: Rotation3d{rvec, units::radian_t{ct_matrix{rvec}.norm()}} {}
/**
* Constructs a Rotation3d from a rotation matrix.
@@ -71,7 +108,63 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix);
constexpr explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
auto impl = []<typename Matrix3d>(const Matrix3d& R) -> Quaternion {
// Require that the rotation matrix is special orthogonal. This is true if
// the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) {
throw std::domain_error("Rotation matrix isn't orthogonal");
}
if (gcem::abs(R.determinant() - 1.0) > 1e-9) {
throw std::domain_error(
"Rotation matrix is orthogonal but not special orthogonal");
}
// Turn rotation matrix into a quaternion
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
double trace = R(0, 0) + R(1, 1) + R(2, 2);
double w;
double x;
double y;
double z;
if (trace > 0.0) {
double s = 0.5 / gcem::sqrt(trace + 1.0);
w = 0.25 / s;
x = (R(2, 1) - R(1, 2)) * s;
y = (R(0, 2) - R(2, 0)) * s;
z = (R(1, 0) - R(0, 1)) * s;
} else {
if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
double s = 2.0 * gcem::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
w = (R(2, 1) - R(1, 2)) / s;
x = 0.25 * s;
y = (R(0, 1) + R(1, 0)) / s;
z = (R(0, 2) + R(2, 0)) / s;
} else if (R(1, 1) > R(2, 2)) {
double s = 2.0 * gcem::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
w = (R(0, 2) - R(2, 0)) / s;
x = (R(0, 1) + R(1, 0)) / s;
y = 0.25 * s;
z = (R(1, 2) + R(2, 1)) / s;
} else {
double s = 2.0 * gcem::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
w = (R(1, 0) - R(0, 1)) / s;
x = (R(0, 2) + R(2, 0)) / s;
y = (R(1, 2) + R(2, 1)) / s;
z = 0.25 * s;
}
}
return Quaternion{w, x, y, z};
};
if (std::is_constant_evaluated()) {
m_q = impl(ct_matrix3d{rotationMatrix});
} else {
m_q = impl(rotationMatrix);
}
}
/**
* Constructs a Rotation3d that rotates the initial vector onto the final
@@ -83,7 +176,58 @@ class WPILIB_DLLEXPORT Rotation3d {
* @param initial The initial vector.
* @param final The final vector.
*/
Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final);
constexpr Rotation3d(const Eigen::Vector3d& initial,
const Eigen::Vector3d& final) {
double dot = ct_matrix{initial}.dot(ct_matrix{final});
double normProduct = ct_matrix{initial}.norm() * ct_matrix{final}.norm();
double dotNorm = dot / normProduct;
if (dotNorm > 1.0 - 1E-9) {
// If the dot product is 1, the two vectors point in the same direction so
// there's no rotation. The default initialization of m_q will work.
return;
} else if (dotNorm < -1.0 + 1E-9) {
// If the dot product is -1, the two vectors are antiparallel, so a 180°
// rotation is required. Any other vector can be used to generate an
// orthogonal one.
double x = gcem::abs(initial.coeff(0));
double y = gcem::abs(initial.coeff(1));
double z = gcem::abs(initial.coeff(2));
// Find vector that is most orthogonal to initial vector
Eigen::Vector3d other;
if (x < y) {
if (x < z) {
// Use x-axis
other = Eigen::Vector3d{{1, 0, 0}};
} else {
// Use z-axis
other = Eigen::Vector3d{{0, 0, 1}};
}
} else {
if (y < z) {
// Use y-axis
other = Eigen::Vector3d{{0, 1, 0}};
} else {
// Use z-axis
other = Eigen::Vector3d{{0, 0, 1}};
}
}
auto axis = ct_matrix{initial}.cross(ct_matrix{other});
double axisNorm = axis.norm();
m_q = Quaternion{0.0, axis(0) / axisNorm, axis(1) / axisNorm,
axis(2) / axisNorm};
} else {
auto axis = ct_matrix{initial}.cross(final);
// https://stackoverflow.com/a/11741520
m_q =
Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
}
}
/**
* Adds two rotations together.
@@ -92,7 +236,9 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @return The sum of the two rotations.
*/
Rotation3d operator+(const Rotation3d& other) const;
constexpr Rotation3d operator+(const Rotation3d& other) const {
return RotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
@@ -102,14 +248,16 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @return The difference between the two rotations.
*/
Rotation3d operator-(const Rotation3d& other) const;
constexpr Rotation3d operator-(const Rotation3d& other) const {
return *this + -other;
}
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
Rotation3d operator-() const;
constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; }
/**
* Multiplies the current rotation by a scalar.
@@ -118,7 +266,16 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator*(double scalar) const;
constexpr Rotation3d operator*(double scalar) const {
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
if (m_q.W() >= 0.0) {
return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}},
2.0 * units::radian_t{scalar * gcem::acos(m_q.W())}};
} else {
return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}},
2.0 * units::radian_t{scalar * gcem::acos(-m_q.W())}};
}
}
/**
* Divides the current rotation by a scalar.
@@ -127,12 +284,17 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator/(double scalar) const;
constexpr Rotation3d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Checks equality between this Rotation3d and another object.
*/
bool operator==(const Rotation3d&) const;
constexpr bool operator==(const Rotation3d& other) const {
return gcem::abs(gcem::abs(m_q.Dot(other.m_q)) -
m_q.Norm() * other.m_q.Norm()) < 1e-9;
}
/**
* Adds the new rotation to the current rotation. The other rotation is
@@ -145,43 +307,98 @@ class WPILIB_DLLEXPORT Rotation3d {
*
* @return The new rotated Rotation3d.
*/
Rotation3d RotateBy(const Rotation3d& other) const;
constexpr Rotation3d RotateBy(const Rotation3d& other) const {
return Rotation3d{other.m_q * m_q};
}
/**
* Returns the quaternion representation of the Rotation3d.
*/
const Quaternion& GetQuaternion() const;
constexpr const Quaternion& GetQuaternion() const { return m_q; }
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
units::radian_t X() const;
constexpr units::radian_t X() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// wpimath/algorithms.md
double cxcy = 1.0 - 2.0 * (x * x + y * y);
double sxcy = 2.0 * (w * x + y * z);
double cy_sq = cxcy * cxcy + sxcy * sxcy;
if (cy_sq > 1e-20) {
return units::radian_t{gcem::atan2(sxcy, cxcy)};
} else {
return 0_rad;
}
}
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
units::radian_t Y() const;
constexpr units::radian_t Y() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
double ratio = 2.0 * (w * y - z * x);
if (gcem::abs(ratio) >= 1.0) {
return units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)};
} else {
return units::radian_t{gcem::asin(ratio)};
}
}
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
units::radian_t Z() const;
constexpr units::radian_t Z() const {
double w = m_q.W();
double x = m_q.X();
double y = m_q.Y();
double z = m_q.Z();
// wpimath/algorithms.md
double cycz = 1.0 - 2.0 * (y * y + z * z);
double cysz = 2.0 * (w * z + x * y);
double cy_sq = cycz * cycz + cysz * cysz;
if (cy_sq > 1e-20) {
return units::radian_t{gcem::atan2(cysz, cycz)};
} else {
return units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)};
}
}
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Eigen::Vector3d Axis() const;
constexpr Eigen::Vector3d Axis() const {
double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
if (norm == 0.0) {
return Eigen::Vector3d{{0.0, 0.0, 0.0}};
} else {
return Eigen::Vector3d{{m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}};
}
}
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
units::radian_t Angle() const;
constexpr units::radian_t Angle() const {
double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z());
return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
}
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.
*/
Rotation2d ToRotation2d() const;
constexpr Rotation2d ToRotation2d() const { return Rotation2d{Z()}; }
private:
Quaternion m_q;

29
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -13,7 +13,7 @@
namespace frc {
class WPILIB_DLLEXPORT Pose2d;
class Pose2d;
/**
* Represents a transformation for a Pose2d in the pose's frame.
@@ -26,7 +26,7 @@ class WPILIB_DLLEXPORT Transform2d {
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform2d(Pose2d initial, Pose2d final);
constexpr Transform2d(const Pose2d& initial, const Pose2d& final);
/**
* Constructs a transform with the given translation and rotation components.
@@ -121,17 +121,38 @@ class WPILIB_DLLEXPORT Transform2d {
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform2d operator+(const Transform2d& other) const;
constexpr Transform2d operator+(const Transform2d& other) const;
/**
* Checks equality between this Transform2d and another object.
*/
bool operator==(const Transform2d&) const = default;
constexpr bool operator==(const Transform2d&) const = default;
private:
Translation2d m_translation;
Rotation2d m_rotation;
};
} // namespace frc
#include "frc/geometry/Pose2d.h"
namespace frc {
constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
constexpr Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
} // namespace frc
#ifndef NO_PROTOBUF

63
wpimath/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -4,13 +4,15 @@
#pragma once
#include <utility>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
class Pose3d;
/**
* Represents a transformation for a Pose3d in the pose's frame.
@@ -23,7 +25,7 @@ class WPILIB_DLLEXPORT Transform3d {
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
constexpr Transform3d(const Pose3d& initial, const Pose3d& final);
/**
* Constructs a transform with the given translation and rotation components.
@@ -31,7 +33,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
constexpr Transform3d(Translation3d translation, Rotation3d rotation)
: m_translation{std::move(translation)},
m_rotation{std::move(rotation)} {}
/**
* Constructs a transform with x, y, and z translations instead of a separate
@@ -42,8 +46,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param z The z component of the translational component of the transform.
* @param rotation The rotational component of the transform.
*/
Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation)
: m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
/**
* Constructs the identity transform -- maps an initial pose to itself.
@@ -55,42 +60,47 @@ class WPILIB_DLLEXPORT Transform3d {
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
constexpr const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
constexpr units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
constexpr units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
constexpr units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
constexpr const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
constexpr Transform3d Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
/**
* Multiplies the transform by the scalar.
@@ -98,7 +108,7 @@ class WPILIB_DLLEXPORT Transform3d {
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
constexpr Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
@@ -108,7 +118,9 @@ class WPILIB_DLLEXPORT Transform3d {
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); }
constexpr Transform3d operator/(double scalar) const {
return *this * (1.0 / scalar);
}
/**
* Composes two transformations. The second transform is applied relative to
@@ -117,17 +129,38 @@ class WPILIB_DLLEXPORT Transform3d {
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
constexpr Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*/
bool operator==(const Transform3d&) const = default;
constexpr bool operator==(const Transform3d&) const = default;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc
#include "frc/geometry/Pose3d.h"
namespace frc {
constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = (final.Translation() - initial.Translation())
.RotateBy(-initial.Rotation());
m_rotation = final.Rotation() - initial.Rotation();
}
constexpr Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
} // namespace frc
#ifndef NO_PROTOBUF

24
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <algorithm>
#include <initializer_list>
#include <span>
@@ -58,7 +59,9 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @param vector The translation vector to represent.
*/
explicit Translation2d(const Eigen::Vector2d& vector);
constexpr explicit Translation2d(const Eigen::Vector2d& vector)
: m_x{units::meter_t{vector.coeff(0)}},
m_y{units::meter_t{vector.coeff(1)}} {}
/**
* Calculates the distance between two translations in 2D space.
@@ -101,7 +104,7 @@ class WPILIB_DLLEXPORT Translation2d {
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); }
/**
* Returns the angle this translation forms with the positive X axis.
@@ -234,15 +237,26 @@ class WPILIB_DLLEXPORT Translation2d {
* @param translations The collection of translations.
* @return The nearest Translation2d from the collection.
*/
Translation2d Nearest(std::span<const Translation2d> translations) const;
constexpr Translation2d Nearest(
std::span<const Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
}
/**
* Returns the nearest Translation2d from a collection of translations
* @param translations The collection of translations.
* @return The nearest Translation2d from the collection.
*/
Translation2d Nearest(
std::initializer_list<Translation2d> translations) const;
constexpr Translation2d Nearest(
std::initializer_list<Translation2d> translations) const {
return *std::min_element(translations.begin(), translations.end(),
[this](Translation2d a, Translation2d b) {
return this->Distance(a) < this->Distance(b);
});
}
private:
units::meter_t m_x = 0_m;

10
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT Translation3d {
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle) {
constexpr Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
m_y = rectangular.Y();
@@ -61,7 +61,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @param vector The translation vector to represent.
*/
explicit Translation3d(const Eigen::Vector3d& vector)
constexpr explicit Translation3d(const Eigen::Vector3d& vector)
: m_x{units::meter_t{vector.x()}},
m_y{units::meter_t{vector.y()}},
m_z{units::meter_t{vector.z()}} {}
@@ -76,7 +76,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const {
constexpr units::meter_t Distance(const Translation3d& other) const {
return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) +
units::math::pow<2>(other.m_y - m_y) +
units::math::pow<2>(other.m_z - m_z));
@@ -117,7 +117,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The norm of the translation.
*/
units::meter_t Norm() const {
constexpr units::meter_t Norm() const {
return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
@@ -131,7 +131,7 @@ class WPILIB_DLLEXPORT Translation3d {
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const {
constexpr Translation3d RotateBy(const Rotation3d& other) const {
Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()};
auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse();
return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()},

4
wpimath/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -11,6 +11,7 @@
#include "units/math.h"
namespace frc {
/**
* A change in distance along a 2D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose2ds from a Twist2d and
@@ -40,7 +41,7 @@ struct WPILIB_DLLEXPORT Twist2d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist2d& other) const {
constexpr bool operator==(const Twist2d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
@@ -56,6 +57,7 @@ struct WPILIB_DLLEXPORT Twist2d {
return Twist2d{dx * factor, dy * factor, dtheta * factor};
}
};
} // namespace frc
#ifndef NO_PROTOBUF

5
wpimath/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -6,12 +6,12 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation3d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
/**
* A change in distance along a 3D arc since the last pose update. We can use
* ideas from differential calculus to create new Pose3ds from a Twist3d and
@@ -56,7 +56,7 @@ struct WPILIB_DLLEXPORT Twist3d {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Twist3d& other) const {
constexpr bool operator==(const Twist3d& other) const {
return units::math::abs(dx - other.dx) < 1E-9_m &&
units::math::abs(dy - other.dy) < 1E-9_m &&
units::math::abs(dz - other.dz) < 1E-9_m &&
@@ -76,6 +76,7 @@ struct WPILIB_DLLEXPORT Twist3d {
rx * factor, ry * factor, rz * factor};
}
};
} // namespace frc
#ifndef NO_PROTOBUF