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

22
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a 2d pose containing translational and rotational elements.
*/
@@ -154,4 +156,24 @@ public class Pose2d {
return this.plus(transform);
}
/**
* Checks equality between this Pose2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose2d) {
return ((Pose2d) obj).m_translation.equals(m_translation)
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

25
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
@@ -118,8 +120,8 @@ public class Rotation2d {
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
m_cos * other.m_cos - m_sin * other.m_sin,
m_cos * other.m_sin + m_sin * other.m_cos
m_cos * other.m_cos - m_sin * other.m_sin,
m_cos * other.m_sin + m_sin * other.m_cos
);
}
@@ -167,4 +169,23 @@ public class Rotation2d {
public double getTan() {
return m_sin / m_cos;
}
/**
* Checks equality between this Rotation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation2d) {
return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_value);
}
}

22
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a transformation for a Pose2d.
*/
@@ -66,4 +68,24 @@ public class Transform2d {
public Rotation2d getRotation() {
return m_rotation;
}
/**
* Checks equality between this Transform2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform2d) {
return ((Transform2d) obj).m_translation.equals(m_translation)
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

22
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a translation in 2d space.
* This object can be used to represent a point or a vector.
@@ -162,4 +164,24 @@ public class Translation2d {
public Translation2d div(double scalar) {
return new Translation2d(m_x / scalar, m_y / scalar);
}
/**
* Checks equality between this Translation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation2d) {
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y);
}
}

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
View File

@@ -7,6 +7,8 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
@@ -45,4 +47,25 @@ public class Twist2d {
this.dy = dy;
this.dtheta = dtheta;
}
/**
* Checks equality between this Twist2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist2d) {
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
View File

@@ -94,8 +94,7 @@ public class MecanumDriveKinematics {
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
// We have a new center of rotation. We need to compute the matrix again.
if (centerOfRotationMeters.getX() != m_prevCoR.getX()
|| centerOfRotationMeters.getY() != m_prevCoR.getY()) {
if (!centerOfRotationMeters.equals(m_prevCoR)) {
var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
View File

@@ -96,8 +96,7 @@ public class SwerveDriveKinematics {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
if (centerOfRotationMeters.getX() != m_prevCoR.getX()
|| centerOfRotationMeters.getY() != m_prevCoR.getY()) {
if (!centerOfRotationMeters.equals(m_prevCoR)) {
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
-m_modules[i].getY() + centerOfRotationMeters.getY());