Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

93
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -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()}};