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

Add equality comparator to geometry classes (#1882)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-09-08 14:20:26 -04:00
committed by Peter Johnson
parent 62f07c182c
commit 86b666bba9
26 changed files with 341 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -27,6 +27,14 @@ Pose2d& Pose2d::operator+=(const Transform2d& other) {
return *this;
}
bool Pose2d::operator==(const Pose2d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Pose2d::operator!=(const Pose2d& other) const {
return !operator==(other);
}
Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};

8
wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -52,6 +52,14 @@ Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
bool Rotation2d::operator==(const Rotation2d& other) const {
return units::math::abs(m_value - other.m_value) < 1E-9_rad;
}
bool Rotation2d::operator!=(const Rotation2d& other) const {
return !operator==(other);
}
Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};

8
wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -23,3 +23,11 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation(translation), m_rotation(rotation) {}
bool Transform2d::operator==(const Transform2d& other) const {
return m_translation == other.m_translation && m_rotation == other.m_rotation;
}
bool Transform2d::operator!=(const Transform2d& other) const {
return !operator==(other);
}

9
wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -60,6 +60,15 @@ Translation2d Translation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;
}
bool Translation2d::operator!=(const Translation2d& other) const {
return !operator==(other);
}
Translation2d& Translation2d::operator/=(double scalar) {
*this *= (1.0 / scalar);
return *this;

3
wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -12,8 +12,7 @@ using namespace frc;
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotation.X() != m_previousCoR.X() ||
centerOfRotation.Y() != m_previousCoR.Y()) {
if (centerOfRotation != m_previousCoR) {
auto fl = m_frontLeftWheel - centerOfRotation;
auto fr = m_frontRightWheel - centerOfRotation;
auto rl = m_rearLeftWheel - centerOfRotation;