From 6e8feb154c2b2a43b4990c47c842f67932a86bd8 Mon Sep 17 00:00:00 2001 From: Wispy <101812473+WispySparks@users.noreply.github.com> Date: Sun, 8 Sep 2024 00:23:19 -0500 Subject: [PATCH] [wpimath] Add more support for wpiunits in the geometry classes (#6587) --- .../wpi/first/math/geometry/Ellipse2d.java | 54 +++++++++++++++ .../edu/wpi/first/math/geometry/Pose2d.java | 18 +++++ .../edu/wpi/first/math/geometry/Pose3d.java | 43 ++++++++++++ .../wpi/first/math/geometry/Rectangle2d.java | 43 ++++++++++++ .../wpi/first/math/geometry/Rotation3d.java | 68 +++++++++++++++++++ .../wpi/first/math/geometry/Transform2d.java | 18 +++++ .../wpi/first/math/geometry/Transform3d.java | 43 ++++++++++++ .../first/math/geometry/Translation2d.java | 18 +++++ .../first/math/geometry/Translation3d.java | 27 ++++++++ 9 files changed, 332 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 499dae8654..a11aa6d7e9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -4,10 +4,13 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.proto.Ellipse2dProto; import edu.wpi.first.math.geometry.struct.Ellipse2dStruct; import edu.wpi.first.math.jni.Ellipse2dJNI; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -35,6 +38,18 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable { m_ySemiAxis = ySemiAxis; } + /** + * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one. + * The X and Y semi-axis will be converted to and tracked as meters. + * + * @param center The center of the ellipse. + * @param xSemiAxis The x semi-axis. + * @param ySemiAxis The y semi-axis. + */ + public Ellipse2d(Pose2d center, Distance xSemiAxis, Distance ySemiAxis) { + this(center, xSemiAxis.in(Meters), ySemiAxis.in(Meters)); + } + /** * Constructs a perfectly circular ellipse with the specified radius. * @@ -45,6 +60,17 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable { this(new Pose2d(center, Rotation2d.kZero), radius, radius); } + /** + * Constructs a perfectly circular ellipse with the specified radius. The radius will be converted + * to and tracked as meters. + * + * @param center The center of the circle. + * @param radius The radius of the circle. + */ + public Ellipse2d(Translation2d center, Distance radius) { + this(new Pose2d(center, Rotation2d.kZero), radius, radius); + } + /** * Returns the center of the ellipse. * @@ -81,6 +107,24 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable { return m_ySemiAxis; } + /** + * Returns the x semi-axis in a measure. + * + * @return The x semi-axis in a measure. + */ + public Distance getMeasureXSemiAxis() { + return Meters.of(m_xSemiAxis); + } + + /** + * Returns the y semi-axis in a measure. + * + * @return The y semi-axis in a measure. + */ + public Distance getMeasureYSemiAxis() { + return Meters.of(m_ySemiAxis); + } + /** * Returns the focal points of the ellipse. In a perfect circle, this will always return the * center. @@ -158,6 +202,16 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable { return findNearestPoint(point).getDistance(point); } + /** + * Returns the distance between the perimeter of the ellipse and the point in a measure. + * + * @param point The point to check. + * @return The distance (0, if the point is contained by the ellipse) in a measure. + */ + public Distance getMeasureDistance(Translation2d point) { + return Meters.of(getDistance(point)); + } + /** * Returns the nearest point that is contained within the ellipse. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 536f44e9e5..89c404fddb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -134,6 +134,24 @@ public class Pose2d implements Interpolatable, ProtobufSerializable, Str return m_translation.getY(); } + /** + * Returns the X component of the pose's translation in a measure. + * + * @return The x component of the pose's translation in a measure. + */ + public Distance getMeasureX() { + return m_translation.getMeasureX(); + } + + /** + * Returns the Y component of the pose's translation in a measure. + * + * @return The y component of the pose's translation in a measure. + */ + public Distance getMeasureY() { + return m_translation.getMeasureY(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 6b116a6cff..ab744a1408 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; @@ -12,6 +14,7 @@ import edu.wpi.first.math.geometry.proto.Pose3dProto; import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -63,6 +66,19 @@ public class Pose3d implements Interpolatable, ProtobufSerializable, Str m_rotation = rotation; } + /** + * Constructs a pose with x, y, and z translations instead of a separate Translation3d. The X, Y, + * and Z translations will be converted to and tracked as meters. + * + * @param x The x component of the translational component of the pose. + * @param y The y component of the translational component of the pose. + * @param z The z component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) { + this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -134,6 +150,33 @@ public class Pose3d implements Interpolatable, ProtobufSerializable, Str return m_translation.getZ(); } + /** + * Returns the X component of the pose's translation in a measure. + * + * @return The x component of the pose's translation in a measure. + */ + public Distance getMeasureX() { + return m_translation.getMeasureX(); + } + + /** + * Returns the Y component of the pose's translation in a measure. + * + * @return The y component of the pose's translation in a measure. + */ + public Distance getMeasureY() { + return m_translation.getMeasureY(); + } + + /** + * Returns the Z component of the pose's translation in a measure. + * + * @return The z component of the pose's translation in a measure. + */ + public Distance getMeasureZ() { + return m_translation.getMeasureZ(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 9541021ffe..3b9455e47b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -4,9 +4,12 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.proto.Rectangle2dProto; import edu.wpi.first.math.geometry.struct.Rectangle2dStruct; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -36,6 +39,18 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { m_yWidth = yWidth; } + /** + * Constructs a rectangle at the specified position with the specified width and height. The X and + * Y widths will be converted to and tracked as meters. + * + * @param center The position (translation and rotation) of the rectangle. + * @param xWidth The x size component of the rectangle, in unrotated coordinate frame. + * @param yWidth The y size component of the rectangle, in unrotated coordinate frame. + */ + public Rectangle2d(Pose2d center, Distance xWidth, Distance yWidth) { + this(center, xWidth.in(Meters), yWidth.in(Meters)); + } + /** * Creates an unrotated rectangle from the given corners. The corners should be diagonally * opposite of each other. @@ -86,6 +101,24 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { return m_yWidth; } + /** + * Returns the X size component of the rectangle in a measure. + * + * @return The x size component of the rectangle in a measure. + */ + public Distance getMeasureXWidth() { + return Meters.of(m_xWidth); + } + + /** + * Returns the Y size component of the rectangle in a measure. + * + * @return The y size component of the rectangle in a measure. + */ + public Distance getMeasureYWidth() { + return Meters.of(m_yWidth); + } + /** * Transforms the center of the rectangle and returns the new rectangle. * @@ -156,6 +189,16 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { return findNearestPoint(point).getDistance(point); } + /** + * Returns the distance between the perimeter of the rectangle and the point in a measure. + * + * @param point The point to check. + * @return The distance (0, if the point is contained by the rectangle) in a measure. + */ + public Distance getMeasureDistance(Translation2d point) { + return Meters.of(getDistance(point)); + } + /** * Returns the nearest point that is contained within the rectangle. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 724c5ed793..123badb6ca 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Radians; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; @@ -18,6 +20,7 @@ import edu.wpi.first.math.geometry.proto.Rotation3dProto; import edu.wpi.first.math.geometry.struct.Rotation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -85,6 +88,24 @@ public class Rotation3d cr * cp * sy - sr * sp * cy); } + /** + * Constructs a Rotation3d from extrinsic roll, pitch, and yaw. + * + *

Extrinsic rotations occur in that order around the axes in the fixed global frame rather + * than the body frame. + * + *

Angles are measured counterclockwise with the rotation axis pointing "out of the page". If + * you point your right thumb along the positive axis direction, your fingers curl in the + * direction of positive rotation. + * + * @param roll The counterclockwise rotation angle around the X axis (roll). + * @param pitch The counterclockwise rotation angle around the Y axis (pitch). + * @param yaw The counterclockwise rotation angle around the Z axis (yaw). + */ + public Rotation3d(Angle roll, Angle pitch, Angle yaw) { + this(roll.in(Radians), pitch.in(Radians), yaw.in(Radians)); + } + /** * Constructs a Rotation3d with the given rotation vector representation. This representation is * equivalent to axis-angle, where the normalized axis is multiplied by the rotation around the @@ -115,6 +136,17 @@ public class Rotation3d m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0)); } + /** + * Constructs a Rotation3d with the given axis-angle representation. The axis doesn't have to be + * normalized. + * + * @param axis The rotation axis. + * @param angle The rotation around the axis. + */ + public Rotation3d(Vector axis, Angle angle) { + this(axis, angle.in(Radians)); + } + /** * Constructs a Rotation3d from a rotation matrix. * @@ -374,6 +406,33 @@ public class Rotation3d } } + /** + * Returns the counterclockwise rotation angle around the X axis (roll) in a measure. + * + * @return The counterclockwise rotation angle around the x axis (roll) in a measure. + */ + public Angle getMeasureX() { + return Radians.of(getX()); + } + + /** + * Returns the counterclockwise rotation angle around the Y axis (pitch) in a measure. + * + * @return The counterclockwise rotation angle around the y axis (pitch) in a measure. + */ + public Angle getMeasureY() { + return Radians.of(getY()); + } + + /** + * Returns the counterclockwise rotation angle around the Z axis (yaw) in a measure. + * + * @return The counterclockwise rotation angle around the z axis (yaw) in a measure. + */ + public Angle getMeasureZ() { + return Radians.of(getZ()); + } + /** * Returns the axis in the axis-angle representation of this rotation. * @@ -400,6 +459,15 @@ public class Rotation3d return 2.0 * Math.atan2(norm, m_q.getW()); } + /** + * Returns the angle in a measure in the axis-angle representation of this rotation. + * + * @return The angle in a measure in the axis-angle representation of this rotation. + */ + public Angle getMeasureAngle() { + return Radians.of(getAngle()); + } + /** * Returns a Rotation2d representing this Rotation3d projected into the X-Y plane. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 4eab093ae4..79dc16939e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -142,6 +142,24 @@ public class Transform2d implements ProtobufSerializable, StructSerializable { return m_translation.getY(); } + /** + * Returns the X component of the transformation's translation in a measure. + * + * @return The x component of the transformation's translation in a measure. + */ + public Distance getMeasureX() { + return m_translation.getMeasureX(); + } + + /** + * Returns the Y component of the transformation's translation in a measure. + * + * @return The y component of the transformation's translation in a measure. + */ + public Distance getMeasureY() { + return m_translation.getMeasureY(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index e7254da92c..b14f2e0ad4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -4,8 +4,11 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.proto.Transform3dProto; import edu.wpi.first.math.geometry.struct.Transform3dStruct; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -64,6 +67,19 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { m_rotation = rotation; } + /** + * Constructs a transform with x, y, and z translations instead of a separate Translation3d. The + * X, Y, and Z translations will be converted to and tracked as meters. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param z The z component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) { + this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { m_translation = Translation3d.kZero; @@ -137,6 +153,33 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { return m_translation.getZ(); } + /** + * Returns the X component of the transformation's translation in a measure. + * + * @return The x component of the transformation's translation in a measure. + */ + public Distance getMeasureX() { + return m_translation.getMeasureX(); + } + + /** + * Returns the Y component of the transformation's translation in a measure. + * + * @return The y component of the transformation's translation in a measure. + */ + public Distance getMeasureY() { + return m_translation.getMeasureY(); + } + + /** + * Returns the Z component of the transformation's translation in a measure. + * + * @return The z component of the transformation's translation in a measure. + */ + public Distance getMeasureZ() { + return m_translation.getMeasureZ(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 031d88429a..e3857eb14e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -129,6 +129,24 @@ public class Translation2d return m_y; } + /** + * Returns the X component of the translation in a measure. + * + * @return The x component of the translation in a measure. + */ + public Distance getMeasureX() { + return Meters.of(m_x); + } + + /** + * Returns the Y component of the translation in a measure. + * + * @return The y component of the translation in a measure. + */ + public Distance getMeasureY() { + return Meters.of(m_y); + } + /** * Returns a vector representation of this translation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 4ae368b3b6..5ea1ab2c02 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -145,6 +145,33 @@ public class Translation3d return m_z; } + /** + * Returns the X component of the translation in a measure. + * + * @return The x component of the translation in a measure. + */ + public Distance getMeasureX() { + return Meters.of(m_x); + } + + /** + * Returns the Y component of the translation in a measure. + * + * @return The y component of the translation in a measure. + */ + public Distance getMeasureY() { + return Meters.of(m_y); + } + + /** + * Returns the Z component of the translation in a measure. + * + * @return The z component of the translation in a measure. + */ + public Distance getMeasureZ() { + return Meters.of(m_z); + } + /** * Returns a vector representation of this translation. *