mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Make geometry classes constexpr (#7222)
This commit is contained in:
Tyler Veness
@@ -1,41 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/geometry/CoordinateAxis.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} {
|
||||
m_axis /= m_axis.norm();
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::N() {
|
||||
static const CoordinateAxis instance{1.0, 0.0, 0.0};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::S() {
|
||||
static const CoordinateAxis instance{-1.0, 0.0, 0.0};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::E() {
|
||||
static const CoordinateAxis instance{0.0, -1.0, 0.0};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::W() {
|
||||
static const CoordinateAxis instance{0.0, 1.0, 0.0};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::U() {
|
||||
static const CoordinateAxis instance{0.0, 0.0, 1.0};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateAxis& CoordinateAxis::D() {
|
||||
static const CoordinateAxis instance{0.0, 0.0, -1.0};
|
||||
return instance;
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/geometry/CoordinateSystem.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/QR>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
CoordinateSystem::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;
|
||||
R.block<3, 1>(0, 0) = positiveX.m_axis;
|
||||
R.block<3, 1>(0, 1) = positiveY.m_axis;
|
||||
R.block<3, 1>(0, 2) = positiveZ.m_axis;
|
||||
|
||||
// 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};
|
||||
}
|
||||
|
||||
const CoordinateSystem& CoordinateSystem::NWU() {
|
||||
static const CoordinateSystem instance{
|
||||
CoordinateAxis::N(), CoordinateAxis::W(), CoordinateAxis::U()};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateSystem& CoordinateSystem::EDN() {
|
||||
static const CoordinateSystem instance{
|
||||
CoordinateAxis::E(), CoordinateAxis::D(), CoordinateAxis::N()};
|
||||
return instance;
|
||||
}
|
||||
|
||||
const CoordinateSystem& CoordinateSystem::NED() {
|
||||
static const CoordinateSystem instance{
|
||||
CoordinateAxis::N(), CoordinateAxis::E(), CoordinateAxis::D()};
|
||||
return instance;
|
||||
}
|
||||
|
||||
Translation3d CoordinateSystem::Convert(const Translation3d& translation,
|
||||
const CoordinateSystem& from,
|
||||
const CoordinateSystem& to) {
|
||||
return translation.RotateBy(from.m_rotation - to.m_rotation);
|
||||
}
|
||||
|
||||
Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
|
||||
const CoordinateSystem& from,
|
||||
const CoordinateSystem& to) {
|
||||
return rotation.RotateBy(from.m_rotation - to.m_rotation);
|
||||
}
|
||||
|
||||
Pose3d CoordinateSystem::Convert(const Pose3d& pose,
|
||||
const CoordinateSystem& from,
|
||||
const CoordinateSystem& to) {
|
||||
return Pose3d{Convert(pose.Translation(), from, to),
|
||||
Convert(pose.Rotation(), from, to)};
|
||||
}
|
||||
|
||||
Transform3d CoordinateSystem::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))};
|
||||
}
|
||||
@@ -8,10 +8,6 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
units::meter_t Ellipse2d::Distance(const Translation2d& point) const {
|
||||
return FindNearestPoint(point).Distance(point);
|
||||
}
|
||||
|
||||
Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const {
|
||||
// Check if already in ellipse
|
||||
if (Contains(point)) {
|
||||
|
||||
@@ -4,101 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Transform2d Pose2d::operator-(const Pose2d& other) const {
|
||||
const auto pose = this->RelativeTo(other);
|
||||
return Transform2d{pose.Translation(), pose.Rotation()};
|
||||
}
|
||||
|
||||
Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
|
||||
const Transform2d transform{other, *this};
|
||||
return {transform.Translation(), transform.Rotation()};
|
||||
}
|
||||
|
||||
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 = std::sin(dtheta);
|
||||
const auto cosTheta = std::cos(dtheta);
|
||||
|
||||
double s, c;
|
||||
if (std::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;
|
||||
}
|
||||
|
||||
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 (std::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}) *
|
||||
std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
|
||||
|
||||
return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}};
|
||||
}
|
||||
|
||||
Pose2d 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 std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
|
||||
std::abs((this->Rotation() - b.Rotation()).Radians().value());
|
||||
}
|
||||
return aDistance < bDistance;
|
||||
});
|
||||
}
|
||||
|
||||
Pose2d 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 std::abs((this->Rotation() - a.Rotation()).Radians().value()) <
|
||||
std::abs((this->Rotation() - b.Rotation()).Radians().value());
|
||||
}
|
||||
return aDistance < bDistance;
|
||||
});
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Pose2d& pose) {
|
||||
json = wpi::json{{"translation", pose.Translation()},
|
||||
{"rotation", pose.Rotation()}};
|
||||
|
||||
@@ -4,181 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <utility>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <wpi/json.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
namespace {
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
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(2), rotation(1)},
|
||||
{rotation(2), 0.0, -rotation(0)},
|
||||
{-rotation(1), rotation(0), 0.0}};
|
||||
}
|
||||
} // namespace
|
||||
|
||||
Pose3d::Pose3d(Translation3d translation, Rotation3d rotation)
|
||||
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
|
||||
|
||||
Pose3d::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)} {}
|
||||
|
||||
Pose3d::Pose3d(const Pose2d& pose)
|
||||
: m_translation{pose.X(), pose.Y(), 0_m},
|
||||
m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {}
|
||||
|
||||
Pose3d Pose3d::operator+(const Transform3d& other) const {
|
||||
return TransformBy(other);
|
||||
}
|
||||
|
||||
Transform3d Pose3d::operator-(const Pose3d& other) const {
|
||||
const auto pose = this->RelativeTo(other);
|
||||
return Transform3d{pose.Translation(), pose.Rotation()};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::operator*(double scalar) const {
|
||||
return Pose3d{m_translation * scalar, m_rotation * scalar};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::operator/(double scalar) const {
|
||||
return *this * (1.0 / scalar);
|
||||
}
|
||||
|
||||
Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
|
||||
return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::TransformBy(const Transform3d& other) const {
|
||||
return {m_translation + (other.Translation().RotateBy(m_rotation)),
|
||||
other.Rotation() + m_rotation};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::RelativeTo(const Pose3d& other) const {
|
||||
const Transform3d transform{other, *this};
|
||||
return {transform.Translation(), transform.Rotation()};
|
||||
}
|
||||
|
||||
Pose3d Pose3d::Exp(const Twist3d& twist) const {
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()};
|
||||
Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()};
|
||||
Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
|
||||
Eigen::Matrix3d omegaSq = omega * omega;
|
||||
double theta = rvec.norm();
|
||||
double thetaSq = theta * theta;
|
||||
|
||||
double A;
|
||||
double B;
|
||||
double C;
|
||||
if (std::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 = std::sin(theta) / theta;
|
||||
B = (1 - std::cos(theta)) / thetaSq;
|
||||
C = (1 - A) / thetaSq;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq;
|
||||
Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq;
|
||||
|
||||
auto 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;
|
||||
}
|
||||
|
||||
Twist3d Pose3d::Log(const Pose3d& end) const {
|
||||
// Implementation from Section 3.2 of https://ethaneade.org/lie.pdf
|
||||
const auto transform = end.RelativeTo(*this);
|
||||
|
||||
Eigen::Vector3d u{transform.X().value(), transform.Y().value(),
|
||||
transform.Z().value()};
|
||||
Eigen::Vector3d rvec =
|
||||
transform.Rotation().GetQuaternion().ToRotationVector();
|
||||
|
||||
Eigen::Matrix3d omega = RotationVectorToMatrix(rvec);
|
||||
Eigen::Matrix3d omegaSq = omega * omega;
|
||||
double theta = rvec.norm();
|
||||
double thetaSq = theta * theta;
|
||||
|
||||
double C;
|
||||
if (std::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 = std::sin(theta) / theta;
|
||||
double B = (1 - std::cos(theta)) / thetaSq;
|
||||
C = (1 - A / (2 * B)) / thetaSq;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d V_inv =
|
||||
Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq;
|
||||
|
||||
Eigen::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)}};
|
||||
}
|
||||
|
||||
Pose2d Pose3d::ToPose2d() const {
|
||||
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Pose3d& pose) {
|
||||
json = wpi::json{{"translation", pose.Translation()},
|
||||
{"rotation", pose.Rotation()}};
|
||||
|
||||
@@ -4,221 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Quaternion.h"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Quaternion::Quaternion(double w, double x, double y, double z)
|
||||
: m_r{w}, m_v{x, y, z} {}
|
||||
|
||||
Quaternion Quaternion::operator+(const Quaternion& other) const {
|
||||
return Quaternion{
|
||||
m_r + other.m_r,
|
||||
m_v(0) + other.m_v(0),
|
||||
m_v(1) + other.m_v(1),
|
||||
m_v(2) + other.m_v(2),
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator-(const Quaternion& other) const {
|
||||
return Quaternion{
|
||||
m_r - other.m_r,
|
||||
m_v(0) - other.m_v(0),
|
||||
m_v(1) - other.m_v(1),
|
||||
m_v(2) - other.m_v(2),
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const double other) const {
|
||||
return Quaternion{
|
||||
m_r * other,
|
||||
m_v(0) * other,
|
||||
m_v(1) * other,
|
||||
m_v(2) * other,
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator/(const double other) const {
|
||||
return Quaternion{
|
||||
m_r / other,
|
||||
m_v(0) / other,
|
||||
m_v(1) / other,
|
||||
m_v(2) / other,
|
||||
};
|
||||
}
|
||||
|
||||
Quaternion Quaternion::operator*(const Quaternion& other) const {
|
||||
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
|
||||
const auto& r1 = m_r;
|
||||
const auto& v1 = m_v;
|
||||
const auto& r2 = other.m_r;
|
||||
const auto& v2 = other.m_v;
|
||||
|
||||
// v₁ x v₂
|
||||
Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2),
|
||||
v2(0) * v1(2) - v1(0) * v2(2),
|
||||
v1(0) * v2(1) - v2(0) * v1(1)};
|
||||
|
||||
// v = r₁v₂ + r₂v₁ + v₁ x v₂
|
||||
Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross;
|
||||
|
||||
return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂
|
||||
r1 * r2 - v1.dot(v2),
|
||||
// v = r₁v₂ + r₂v₁ + v₁ x v₂
|
||||
v(0), v(1), v(2)};
|
||||
}
|
||||
|
||||
bool Quaternion::operator==(const Quaternion& other) const {
|
||||
return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 &&
|
||||
std::abs(Norm() - other.Norm()) < 1e-9;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Conjugate() const {
|
||||
return Quaternion{W(), -X(), -Y(), -Z()};
|
||||
}
|
||||
|
||||
double Quaternion::Dot(const Quaternion& other) const {
|
||||
return W() * other.W() + m_v.dot(other.m_v);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Inverse() const {
|
||||
double norm = Norm();
|
||||
return Conjugate() / (norm * norm);
|
||||
}
|
||||
|
||||
double Quaternion::Norm() const {
|
||||
return std::sqrt(Dot(*this));
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Normalize() const {
|
||||
double norm = Norm();
|
||||
if (norm == 0.0) {
|
||||
return Quaternion{};
|
||||
} else {
|
||||
return Quaternion{W(), X(), Y(), Z()} / norm;
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Pow(const double other) const {
|
||||
return (Log() * other).Exp();
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Exp(const Quaternion& other) const {
|
||||
return other.Exp() * *this;
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Exp() const {
|
||||
double scalar = std::exp(m_r);
|
||||
|
||||
double axial_magnitude = m_v.norm();
|
||||
double cosine = std::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 = std::sin(axial_magnitude) / axial_magnitude;
|
||||
}
|
||||
|
||||
return Quaternion(cosine * scalar, X() * axial_scalar * scalar,
|
||||
Y() * axial_scalar * scalar, Z() * axial_scalar * scalar);
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Log(const Quaternion& other) const {
|
||||
return (other * Inverse()).Log();
|
||||
}
|
||||
|
||||
Quaternion Quaternion::Log() const {
|
||||
double norm = Norm();
|
||||
double scalar = std::log(norm);
|
||||
|
||||
double v_norm = m_v.norm();
|
||||
|
||||
double s_norm = W() / norm;
|
||||
|
||||
if (std::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 around y = 0 = 1/x -
|
||||
// y^2/3*x^3 + O(y^4)
|
||||
v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W());
|
||||
} else {
|
||||
v_scalar = std::atan2(v_norm, W()) / v_norm;
|
||||
}
|
||||
|
||||
return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1),
|
||||
v_scalar * m_v(2)};
|
||||
}
|
||||
|
||||
double Quaternion::W() const {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
double Quaternion::X() const {
|
||||
return m_v(0);
|
||||
}
|
||||
|
||||
double Quaternion::Y() const {
|
||||
return m_v(1);
|
||||
}
|
||||
|
||||
double Quaternion::Z() const {
|
||||
return m_v(2);
|
||||
}
|
||||
|
||||
Eigen::Vector3d Quaternion::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 = m_v.norm();
|
||||
|
||||
if (norm < 1e-9) {
|
||||
return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v;
|
||||
} else {
|
||||
if (W() < 0.0) {
|
||||
return 2.0 * std::atan2(-norm, -W()) / norm * m_v;
|
||||
} else {
|
||||
return 2.0 * std::atan2(norm, W()) / norm * m_v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) {
|
||||
// 𝑣⃗ = θ * v̂
|
||||
// v̂ = 𝑣⃗ / θ
|
||||
|
||||
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂
|
||||
// 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗
|
||||
|
||||
double theta = rvec.norm();
|
||||
double cos = std::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 = std::sin(theta / 2) / theta;
|
||||
}
|
||||
|
||||
return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1),
|
||||
axial_scalar * rvec(2)};
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
|
||||
json = wpi::json{{"W", quaternion.W()},
|
||||
{"X", quaternion.X()},
|
||||
|
||||
@@ -4,14 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Rotation2d.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
|
||||
json = wpi::json{{"radians", rotation.Radians().value()}};
|
||||
}
|
||||
|
||||
@@ -4,257 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Rotation3d.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
#include <string>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/LU>
|
||||
#include <Eigen/QR>
|
||||
#include <wpi/json.h>
|
||||
|
||||
#include "frc/fmt/Eigen.h"
|
||||
#include "units/math.h"
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Rotation3d::Rotation3d(const Quaternion& q) {
|
||||
m_q = q.Normalize();
|
||||
}
|
||||
|
||||
Rotation3d::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};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Eigen::Vector3d& rvec)
|
||||
: Rotation3d{rvec, units::radian_t{rvec.norm()}} {}
|
||||
|
||||
Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) {
|
||||
double norm = axis.norm();
|
||||
if (norm == 0.0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition
|
||||
Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0);
|
||||
m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) {
|
||||
const auto& R = rotationMatrix;
|
||||
|
||||
// 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() - Eigen::Matrix3d::Identity()).norm() > 1e-9) {
|
||||
std::string msg =
|
||||
fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R);
|
||||
|
||||
wpi::math::MathSharedStore::ReportError(msg);
|
||||
throw std::domain_error(msg);
|
||||
}
|
||||
if (std::abs(R.determinant() - 1.0) > 1e-9) {
|
||||
std::string msg = fmt::format(
|
||||
"Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n",
|
||||
R);
|
||||
|
||||
wpi::math::MathSharedStore::ReportError(msg);
|
||||
throw std::domain_error(msg);
|
||||
}
|
||||
|
||||
// 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 / std::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 * std::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 * std::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 * std::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;
|
||||
}
|
||||
}
|
||||
|
||||
m_q = Quaternion{w, x, y, z};
|
||||
}
|
||||
|
||||
Rotation3d::Rotation3d(const Eigen::Vector3d& initial,
|
||||
const Eigen::Vector3d& final) {
|
||||
double dot = initial.dot(final);
|
||||
double normProduct = initial.norm() * 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 point in opposite directions so
|
||||
// a 180 degree rotation is required. Any orthogonal vector can be used for
|
||||
// it. Q in the QR decomposition is an orthonormal basis, so it contains
|
||||
// orthogonal unit vectors.
|
||||
Eigen::Matrix<double, 3, 1> X = initial;
|
||||
Eigen::HouseholderQR<decltype(X)> qr{X};
|
||||
Eigen::Matrix<double, 3, 3> Q = qr.householderQ();
|
||||
|
||||
// w = std::cos(θ/2) = std::cos(90°) = 0
|
||||
//
|
||||
// For x, y, and z, we use the second column of Q because the first is
|
||||
// parallel instead of orthogonal. The third column would also work.
|
||||
m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)};
|
||||
} else {
|
||||
// initial x final
|
||||
Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2),
|
||||
final(0) * initial(2) - initial(0) * final(2),
|
||||
initial(0) * final(1) - final(0) * initial(1)};
|
||||
|
||||
// https://stackoverflow.com/a/11741520
|
||||
m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize();
|
||||
}
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator+(const Rotation3d& other) const {
|
||||
return RotateBy(other);
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator-(const Rotation3d& other) const {
|
||||
return *this + -other;
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator-() const {
|
||||
return Rotation3d{m_q.Inverse()};
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator*(double scalar) const {
|
||||
// https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
|
||||
if (m_q.W() >= 0.0) {
|
||||
return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()},
|
||||
2.0 * units::radian_t{scalar * std::acos(m_q.W())}};
|
||||
} else {
|
||||
return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()},
|
||||
2.0 * units::radian_t{scalar * std::acos(-m_q.W())}};
|
||||
}
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::operator/(double scalar) const {
|
||||
return *this * (1.0 / scalar);
|
||||
}
|
||||
|
||||
bool Rotation3d::operator==(const Rotation3d& other) const {
|
||||
return std::abs(std::abs(m_q.Dot(other.m_q)) -
|
||||
m_q.Norm() * other.m_q.Norm()) < 1e-9;
|
||||
}
|
||||
|
||||
Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const {
|
||||
return Rotation3d{other.m_q * m_q};
|
||||
}
|
||||
|
||||
const Quaternion& Rotation3d::GetQuaternion() const {
|
||||
return m_q;
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::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{std::atan2(sxcy, cxcy)};
|
||||
} else {
|
||||
return 0_rad;
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::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 (std::abs(ratio) >= 1.0) {
|
||||
return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)};
|
||||
} else {
|
||||
return units::radian_t{std::asin(ratio)};
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::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{std::atan2(cysz, cycz)};
|
||||
} else {
|
||||
return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)};
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Vector3d Rotation3d::Axis() const {
|
||||
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
|
||||
if (norm == 0.0) {
|
||||
return {0.0, 0.0, 0.0};
|
||||
} else {
|
||||
return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm};
|
||||
}
|
||||
}
|
||||
|
||||
units::radian_t Rotation3d::Angle() const {
|
||||
double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z());
|
||||
return units::radian_t{2.0 * std::atan2(norm, m_q.W())};
|
||||
}
|
||||
|
||||
Rotation2d Rotation3d::ToRotation2d() const {
|
||||
return Rotation2d{Z()};
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
|
||||
json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
|
||||
}
|
||||
|
||||
@@ -1,23 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/geometry/Transform2d.h"
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Transform2d::Transform2d(Pose2d initial, 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();
|
||||
}
|
||||
|
||||
Transform2d Transform2d::operator+(const Transform2d& other) const {
|
||||
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
|
||||
}
|
||||
@@ -1,39 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/geometry/Transform3d.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "frc/geometry/Pose3d.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Transform3d::Transform3d(Pose3d initial, 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();
|
||||
}
|
||||
|
||||
Transform3d::Transform3d(Translation3d translation, Rotation3d rotation)
|
||||
: m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {}
|
||||
|
||||
Transform3d::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)} {}
|
||||
|
||||
Transform3d 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()};
|
||||
}
|
||||
|
||||
Transform3d Transform3d::operator+(const Transform3d& other) const {
|
||||
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
|
||||
}
|
||||
@@ -4,37 +4,8 @@
|
||||
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
Translation2d::Translation2d(const Eigen::Vector2d& vector)
|
||||
: m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
|
||||
|
||||
units::meter_t Translation2d::Norm() const {
|
||||
return units::math::hypot(m_x, m_y);
|
||||
}
|
||||
|
||||
Translation2d 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);
|
||||
});
|
||||
}
|
||||
|
||||
Translation2d 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);
|
||||
});
|
||||
}
|
||||
|
||||
void frc::to_json(wpi::json& json, const Translation2d& translation) {
|
||||
json =
|
||||
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
|
||||
|
||||
@@ -6,8 +6,6 @@
|
||||
|
||||
#include <wpi/json.h>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
void frc::to_json(wpi::json& json, const Translation3d& translation) {
|
||||
json = wpi::json{{"x", translation.X().value()},
|
||||
{"y", translation.Y().value()},
|
||||
|
||||
Reference in New Issue
Block a user