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

[wpimath] Add more support for wpiunits in the geometry classes (#6587)

Browse Source
This commit is contained in:
Wispy
2024-09-08 00:23:19 -05:00
committed by GitHub
parent 7f6f19eef4
commit 6e8feb154c
9 changed files with 332 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
View File

@@ -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.
*

18
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -134,6 +134,24 @@ public class Pose2d implements Interpolatable<Pose2d>, 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.
*

43
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -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<Pose3d>, 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<Pose3d>, 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.
*

43
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
View File

@@ -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.
*

68
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -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.
*
* <p>Extrinsic rotations occur in that order around the axes in the fixed global frame rather
* than the body frame.
*
* <p>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<N3> 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.
*

18
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -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.
*

43
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -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.
*

18
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -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.
*

27
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -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.
*